Merge remote-tracking branch 'px4/main' into pr-icm42688p_sensor_config

.vscode/settings.json

@@ -2,7 +2,6 @@
         "astyle.astylerc": "${workspaceFolder}/Tools/astyle/astylerc",
         "astyle.c.enable": true,
         "astyle.cpp.enable": true,
-        "breadcrumbs.enabled": true,
         "C_Cpp.autoAddFileAssociations": false,
         "C_Cpp.clang_format_fallbackStyle": "none",
         "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": true,
@@ -20,7 +19,6 @@
         "cmakeExplorer.buildDir": "${workspaceFolder}/build/px4_sitl_test",
         "cmakeExplorer.parallelJobs": 1,
         "cmakeExplorer.suiteDelimiter": "-",
-        "cortex-debug.enableTelemetry": false,
         "cSpell.allowCompoundWords": true,
         "cSpell.diagnosticLevel": "Hint",
         "cSpell.showStatus": false,
@@ -31,7 +29,6 @@
         ],
         "debug.toolBarLocation": "docked",
         "editor.defaultFormatter": "chiehyu.vscode-astyle",
-        "editor.dragAndDrop": false,
         "editor.insertSpaces": false,
         "editor.minimap.maxColumn": 120,
         "editor.minimap.renderCharacters": false,
@@ -127,12 +124,7 @@
                 "${workspaceFolder}/build": true
         },
         "search.showLineNumbers": true,
-        "telemetry.enableTelemetry": false,
-        "terminal.integrated.scrollback": 5000,
-        "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}",
-        "workbench.editor.highlightModifiedTabs": true,
-        "workbench.enableExperiments": false,
-        "workbench.settings.enableNaturalLanguageSearch": false,
+        "terminal.integrated.scrollback": 15000,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         }

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -49,7 +49,7 @@ param set-default CA_AIRFRAME 4
 
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_PX 0
-param set-default CA_ROTOR0_PY 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX 0
 param set-default CA_ROTOR1_PY -1

ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision

@@ -5,12 +5,12 @@
 # @type Quadrotor Wide
 #
 
-. ${R}etc/init.d-posix/airframes/10016_iris
+. ${R}etc/init.d-posix/airframes/10016_gazebo-classic_iris
 
-# EKF2: Vision position and heading
-param set-default EKF2_AID_MASK 24
+# EKF2: Vision position and heading, no GPS
 param set-default EKF2_EV_DELAY 5
 param set-default EKF2_EV_CTRL 15
+param set-default EKF2_HGT_REF 3
 param set-default EKF2_GPS_CTRL 0
 
 # LPE: Vision + baro

ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane

@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam

@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult

@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal

@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
 
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05

ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric

@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
 
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05

ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod

@@ -10,8 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 15
-
 param set-default FW_P_TC 0.5
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -22,7 +20,7 @@ param set-default FW_RR_FF 0.20
 param set-default FW_RR_I 0.02
 param set-default FW_RR_P 0.22
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_W_EN 1
 

ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo

@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
 
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05

ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer

@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2

ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider

@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal

@@ -11,7 +11,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
 
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05

ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane

@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_ANG 8
 param set-default FW_THR_LND_MAX 0
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_MAN_P_MAX 30
 

ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol

@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -13,7 +13,7 @@ param set-default CA_AIRFRAME 4
 
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1
-param set-default CA_ROTOR0_PY 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@@ -42,7 +42,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2

ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor

@@ -46,7 +46,7 @@ param set-default PWM_MAIN_FUNC7 201
 param set-default PWM_MAIN_FUNC8 202
 param set-default PWM_MAIN_FUNC9 203
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2

ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop

@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2

ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar

@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -15,7 +15,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_ANG 8
 
-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -54,6 +54,8 @@ param set-default RWTO_TKOFF 1
 
 param set-default CA_AIRFRAME 1
 
+param set-default COM_PREARM_MODE 2
+
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
 

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -0,0 +1,104 @@
+#!/bin/sh
+#
+# @name Standard VTOL
+#
+# @type Standard VTOL
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
+
+# TODO: Enable motor failure detection when the
+# VTOL no longer reports 0A for all ESCs in SITL
+param set-default FD_ACT_EN 0
+param set-default FD_ACT_MOT_TOUT 500
+
+param set-default CA_AIRFRAME 2
+
+param set-default COM_PREARM_MODE 2
+
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.1515
+param set-default CA_ROTOR0_PY 0.245
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.1515
+param set-default CA_ROTOR1_PY -0.1875
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.1515
+param set-default CA_ROTOR2_PY -0.245
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.1515
+param set-default CA_ROTOR3_PY 0.1875
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_FUNC4 104
+param set-default SIM_GZ_EC_MIN4 10
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default SIM_GZ_EC_FUNC5 105
+param set-default SIM_GZ_EC_MIN5 0
+param set-default SIM_GZ_EC_MAX5 3500
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202
+param set-default SIM_GZ_SV_FUNC3 203
+
+param set-default COM_RC_IN_MODE 1
+param set-default ASPD_PRIMARY 1
+
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+
+param set-default FW_L1_PERIOD 12
+param set-default FW_PR_FF 0.2
+param set-default FW_PR_P 0.9
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MIN -15
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.25
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default MC_AIRMODE 1
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_YAW_P 1.6
+
+param set-default MIS_TAKEOFF_ALT 10
+
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_XY_P 0.8
+param set-default MPC_XY_VEL_P_ACC 3
+param set-default MPC_XY_VEL_I_ACC 4
+param set-default MPC_XY_VEL_D_ACC 0.1
+
+param set-default NAV_ACC_RAD 5
+
+param set-default VT_FWD_THRUST_EN 4
+param set-default VT_F_TRANS_THR 0.75
+param set-default VT_B_TRANS_DUR 8
+param set-default VT_TYPE 2
+param set-default FD_ESCS_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -73,6 +73,7 @@ px4_add_romfs_files(
 	4001_gz_x500
 	4002_gz_x500_depth
 	4003_gz_rc_cessna
+	4004_gz_standard_vtol
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

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

@@ -80,6 +80,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+			sensor_airspeed_sim start
 
 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -93,6 +94,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+			sensor_airspeed_sim start
 
 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -107,6 +109,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+			sensor_airspeed_sim start
 
 		else
 			echo "ERROR [init] gz_bridge failed to start"

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -184,6 +184,9 @@ param set-default SDLOG_DIRS_MAX 7
 param set-default TRIG_INTERFACE 3
 
 param set-default SYS_FAILURE_EN 1
+# Enable low-battery actions by default for (automated) testing. Battery sim
+# does not go below 50% by default, but failure injection can trigger failsafes.
+param set-default COM_LOW_BAT_ACT 2
 
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -38,7 +38,7 @@ param set-default EKF2_GPS_P_GATE 10
 param set-default EKF2_GPS_V_GATE 10
 
 param set-default FW_ARSP_MODE 1
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18
 param set-default FW_PR_P 0.15

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

@@ -30,7 +30,7 @@ param set-default BAT1_N_CELLS 6
 param set-default FW_AIRSPD_MAX 30
 param set-default FW_AIRSPD_MIN 19
 param set-default FW_AIRSPD_TRIM 23
-param set-default FW_L1_R_SLEW_MAX 40
+param set-default FW_PN_R_SLEW_MAX 40
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -20,7 +20,7 @@ control_allocator start
 #
 fw_rate_control start
 fw_att_control start
-fw_pos_control_l1 start
+fw_path_navigation start
 airspeed_selector start
 
 #

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -37,7 +37,7 @@ fi
 
 fw_rate_control start vtol
 fw_att_control start vtol
-fw_pos_control_l1 start vtol
+fw_path_navigation start vtol
 fw_autotune_attitude_control start vtol
 
 # Start Land Detector

Tools/simulation/gz/models/rc_cessna/model.sdf

@@ -685,6 +685,7 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_0</joint_name>
       <sub_topic>servo_0</sub_topic>
+      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.05984281113</a0>
@@ -708,6 +709,7 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_1</joint_name>
       <sub_topic>servo_1</sub_topic>
+      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.05984281113</a0>
@@ -767,6 +769,7 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_2</joint_name>
       <sub_topic>servo_2</sub_topic>
+      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.0</a0>
@@ -789,6 +792,8 @@
     <plugin
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>rudder_joint</joint_name>
+      <sub_topic>servo_3</sub_topic>
+      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
       <jointName>rotor_puller_joint</jointName>

Tools/simulation/gz/models/standard_vtol/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Standard VTOL</name>
+  <version>1.0</version>
+  <sdf version='1.10'>model.sdf</sdf>
+
+  <author>
+   <name>Roman Bapst</name>
+   <email>roman@px4.io</email>
+  </author>
+
+  <description>
+    This is a model of a standard VTOL quad plane.
+  </description>
+</model>

Tools/simulation/gz/models/standard_vtol/model.sdf

@@ -0,0 +1,748 @@
+<?xml version="1.0"?>
+<!-- DO NOT EDIT: Generated from standard_vtol.sdf.jinja -->
+<sdf version='1.10'>
+  <model name='standard_vtol'>
+    <pose>0 0 0.246 0 0 0</pose>
+    <link name='base_link'>
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>5</mass>
+        <inertia>
+          <ixx>0.477708333333</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.341666666667</iyy>
+          <iyz>0</iyz>
+          <izz>0.811041666667</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose>0 0 -0.07 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.55 2.144 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <kp>100000</kp>
+              <kd>1.0</kd>
+              <max_vel>0.1</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose>0.53 -1.072 -0.1 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='left_motor_column'>
+        <pose>0 0.35 0.01 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.74 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='right_motor_column'>
+        <pose>0 -0.35 0.01 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.74 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m0'>
+        <pose>-0.35 0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m1'>
+        <pose>-0.35 -0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m2'>
+        <pose>0.35 -0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m3'>
+        <pose>0.35 0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+    </link>
+    <link name='rotor_0'>
+      <pose>0.35 -0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_0_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.1</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_0_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_0_joint' type='revolute'>
+      <child>rotor_0</child>
+      <parent>base_link</parent>
+      <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'>
+      <pose>-0.35 0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_1_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.1</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_1_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_1_joint' type='revolute'>
+      <child>rotor_1</child>
+      <parent>base_link</parent>
+      <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'>
+      <pose>0.35 0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_2_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.1</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_2_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_2_joint' type='revolute'>
+      <child>rotor_2</child>
+      <parent>base_link</parent>
+      <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'>
+      <pose>-0.35 -0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_3_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.1</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_3_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_3_joint' type='revolute'>
+      <child>rotor_3</child>
+      <parent>base_link</parent>
+      <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_puller'>
+      <pose>-0.22 0 0.0 0 1.57 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_puller_collision'>
+        <pose>0.0 0 -0.04 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_puller_visual'>
+        <pose>0 0 -0.04 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8 0.8 0.8</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_puller_joint' type='revolute'>
+      <pose>0.0 0 0.0 0 -1.57 0</pose>
+      <child>rotor_puller</child>
+      <parent>base_link</parent>
+      <axis>
+        <xyz>1 0 0</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="left_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_elevon_visual'>
+        <pose>-0.105 0.004 -0.034 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="right_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.6 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_elevon_visual'>
+        <pose>0.281 -1.032 -0.034 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="elevator">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose> -0.5 0 0 0.00 0 0.0</pose>
+      </inertial>
+    </link>
+    <joint name='servo_0' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_elevon</child>
+      <pose>-0.18 0.6 -0.005 0 0 0.265</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_1' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_elevon</child>
+      <pose>-0.18 -0.6 -0.005 0 0 -0.265</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_2' type='revolute'>
+      <parent>base_link</parent>
+      <child>elevator</child>
+      <pose> -0.5 0 0 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 0.3 0.05</cp>
+      <area>0.50</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_0</control_joint_name>
+      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_0</joint_name>
+      <sub_topic>servo_0</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 -0.3 0.05</cp>
+      <area>0.50</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_1</control_joint_name>
+      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_1</joint_name>
+      <sub_topic>servo_1</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>-0.2</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.5 0 0</cp>
+      <area>0.01</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_2</control_joint_name>
+          <control_joint_name>servo_2</control_joint_name>
+      <control_joint_rad_to_cl>-12.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_2</joint_name>
+      <sub_topic>servo_2</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_puller_joint</jointName>
+      <linkName>rotor_puller</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>3500</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.01</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>4</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <static>0</static>
+  </model>
+</sdf>

boards/airmind/mindpx-v2/default.px4board

@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/ark/fmu-v6x/default.px4board

@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/av/x-v1/default.px4board

@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/beaglebone/blue/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cuav/nora/default.px4board

@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cuav/x7pro/default.px4board

@@ -51,7 +51,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cubepilot/cubeorange/default.px4board

@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -67,7 +67,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y

boards/cubepilot/cubeorangeplus/default.px4board

@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cubepilot/cubeyellow/default.px4board

@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/emlid/navio2/default.px4board

@@ -33,7 +33,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/durandal-v1/default.px4board

@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/kakuteh7/default.px4board

@@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/kakuteh7mini/default.px4board

@@ -38,7 +38,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/kakuteh7v2/default.px4board

@@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/pix32v5/default.px4board

@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/matek/h743-mini/default.px4board

@@ -29,7 +29,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/matek/h743-slim/default.px4board

@@ -30,7 +30,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/matek/h743/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/modalai/fc-v1/default.px4board

@@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/modalai/fc-v2/default.px4board

@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-classic/default.px4board

@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-f7-oem/default.px4board

@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-f7/default.px4board

@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-h7-oem/default.px4board

@@ -31,7 +31,6 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -43,7 +42,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,7 +50,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -98,4 +96,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/mro/ctrl-zero-h7/default.px4board

@@ -32,7 +32,6 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -44,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -52,7 +51,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -99,4 +97,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/mro/pixracerpro/default.px4board

@@ -31,7 +31,6 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -43,7 +42,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,7 +50,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y

boards/mro/x21-777/default.px4board

@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/x21/default.px4board

@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/nxp/fmuk66-e/default.px4board

@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/nxp/fmuk66-v3/default.px4board

@@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/nxp/fmuk66-v3/test.px4board

@@ -2,6 +2,7 @@ CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y

boards/nxp/mr-canhubk3/fmu.px4board

@@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/nxp/mr-canhubk3/sysview.px4board

@@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/px4/fmu-v2/default.px4board

@@ -35,6 +35,8 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MFT=y

boards/px4/fmu-v2/fixedwing.px4board

@@ -9,5 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y

boards/px4/fmu-v3/default.px4board

@@ -49,7 +49,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v4/default.px4board

@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v4pro/default.px4board

@@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v4pro/test.px4board

@@ -3,6 +3,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y

boards/px4/fmu-v5/default.px4board

@@ -55,7 +55,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v5/protected.px4board

@@ -18,7 +18,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL_L1=n
+CONFIG_MODULES_FW_PATH_NAVIGATION=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n

boards/px4/fmu-v5x/default.px4board

@@ -58,7 +58,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v6c/default.px4board

@@ -42,7 +42,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v6u/default.px4board

@@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v6x/default.px4board

@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/raspberrypi/default.px4board

@@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/sitl/default.px4board

@@ -17,7 +17,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/scumaker/pilotpi/default.px4board

@@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/sky-drones/smartap-airlink/default.px4board

@@ -35,7 +35,6 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -47,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -55,7 +54,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -101,4 +99,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/thepeach/k1/default.px4board

@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/thepeach/r1/default.px4board

@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

msg/CMakeLists.txt

@@ -126,6 +126,7 @@ set(msg_files
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
+	ModeCompleted.msg
 	NavigatorMissionItem.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg

msg/EstimatorAidSource3d.msg

@@ -18,6 +18,4 @@ bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
-# TOPICS estimator_aid_src_ev_vel
-# TOPICS estimator_aid_src_gnss_vel
-# TOPICS estimator_aid_src_mag
+# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag

msg/EstimatorInnovations.msg

@@ -28,6 +28,7 @@ float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance compute
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)
 float32[3] mag_field	# earth magnetic field innovation (Gauss) and innovation variance (Gauss**2)
+float32[3] gravity	# gravity innovation from accelerometerr vector (m/s**2)
 float32[2] drag		# drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2)
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)

msg/EstimatorStatusFlags.msg

@@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on win
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 bool cs_fake_pos                # 32 - true when fake position measurements are being fused
 bool cs_fake_hgt                # 33 - true when fake height measurements are being fused
+bool cs_gravity_vector          # 34 - true when gravity vector measurements are being fused
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/ModeCompleted.msg

@@ -0,0 +1,14 @@
+# Mode completion result, published by an active mode.
+# Note that this is not always published (e.g. when a user switches modes or on
+# failsafe activation)
+uint64 timestamp				 # time since system start (microseconds)
+
+
+uint8 RESULT_SUCCESS = 0
+# [1-99]: reserved
+uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error)
+
+uint8 result                     # One of RESULT_*
+
+uint8 nav_state                  # Source mode
+

platforms/common/include/px4_platform_common/log.h

@@ -71,7 +71,7 @@ __END_DECLS
  * Messages that should never be filtered or compiled out
  ****************************************************************************/
 #define PX4_INFO(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
-#define PX4_INFO_RAW(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_INFO, FMT, ##__VA_ARGS__)
+#define PX4_INFO_RAW(FMT, ...) 	qurt_log_raw(FMT, ##__VA_ARGS__)
 
 #if defined(TRACE_BUILD)
 /****************************************************************************

platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(../stm32_common/io_pins io_pins)
+add_subdirectory(../stm32_common/spi spi)
 add_subdirectory(../stm32_common/hrt hrt)
 add_subdirectory(../stm32_common/board_critmon board_critmon)
 add_subdirectory(../stm32_common/board_reset board_reset)

platforms/qurt/include/qurt_log.h

@@ -57,4 +57,14 @@ static __inline void qurt_log(int level, const char *file, int line,
 	qurt_log_to_apps(level, buf);
 }
 
+static __inline void qurt_log_raw(const char *format, ...)
+{
+	char buf[256];
+	va_list args;
+	va_start(args, format);
+	vsnprintf(buf, sizeof(buf), format, args);
+	va_end(args);
+	qurt_log_to_apps(1, buf);
+}
+
 __END_DECLS

posix-configs/SITL/init/test/test_shutdown

@@ -29,7 +29,7 @@ flight_mode_manager start
 mc_pos_control start vtol
 mc_att_control start vtol
 mc_rate_control start vtol
-fw_pos_control_l1 start vtol
+fw_path_navigation start vtol
 fw_att_control start vtol
 airspeed_selector start
 
@@ -59,7 +59,7 @@ flight_mode_manager status
 mc_pos_control status
 mc_att_control status
 mc_rate_control status
-fw_pos_control_l1 status
+fw_path_navigation status
 fw_att_control status
 airspeed_selector status
 dataman status
@@ -74,7 +74,7 @@ mc_att_control stop
 fw_att_control stop
 flight_mode_manager stop
 mc_pos_control stop
-fw_pos_control_l1 stop
+fw_path_navigation stop
 navigator stop
 commander stop
 land_detector stop

posix-configs/bbblue/px4_fw.config

@@ -56,7 +56,7 @@ ekf2 start
 land_detector start fixedwing
 
 fw_att_control start
-fw_pos_control_l1 start
+fw_path_navigation start
 
 mavlink start -n SoftAp -x -u 14556 -r 1000000 -p
 # -n name of wifi interface: SoftAp, wlan or your custom interface,

posix-configs/rpi/pilotpi_fw.config

@@ -63,7 +63,7 @@ airspeed_selector start
 land_detector start fixedwing
 flight_mode_manager start
 fw_att_control start
-fw_pos_control_l1 start
+fw_path_navigation start
 
 mavlink start -x -u 14556 -r 1000000 -p
 

posix-configs/rpi/px4_fw.config

@@ -41,7 +41,7 @@ navigator start
 ekf2 start
 land_detector start fixedwing
 fw_att_control start
-fw_pos_control_l1 start
+fw_path_navigation start
 
 mavlink start -x -u 14556 -r 1000000 -p
 

src/drivers/imu/bosch/bmi088/BMI088.hpp

@@ -73,6 +73,7 @@ protected:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};