[WIP] logger priority boost command line

* rtl: remove unconditional transition to land after descent

This was a bug, as it renders the above code lines useless.
This would cause a undesired FW landing for VTOL vehicles if
RTL_LAND_DELAY is above 0.

* rtl: head to center after loiter in VTOL FW

To get the same behavior for RTL with and without loiter before land for
VTOL drones.

* rtl: always go to descend state after return

Previously, the state would change directly to land if in MR and
RTL_LAND_DELAY was 0.0, but we will still wish to descent to
RTL_DESCEND_ALT at descent speed, instead of using landing speeds.

* rtl: mark head to center state as part of vtol transition

The next step in the sequence is transition to MC. By setting
vtol_back_transition we ensure that the acceptance radius is adapted to
the expected transition distance.

.ci/Jenkinsfile-compile

@@ -62,6 +62,8 @@ pipeline {
                      "holybro_durandal-v1_default",
                      "holybro_kakutef7_default",
                      "holybro_kakuteh7_default",
+                     "holybro_kakuteh7v2_default",
+                     "holybro_kakuteh7mini_default",
                      "holybro_pix32v5_default",
                      "matek_gnss-m9n-f4_canbootloader",
                      "matek_gnss-m9n-f4_default",

.vscode/cmake-variants.yaml

@@ -180,7 +180,7 @@ CONFIG:
       short: cubepilot_cubeorange
       buildType: MinSizeRel
       settings:
-        CONFIG: cubepilot_orange_test
+        CONFIG: cubepilot_cubeorange_test
     emlid_navio2_default:
       short: emlid_navio2
       buildType: MinSizeRel

.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/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,9 @@ param set-default RWTO_TKOFF 1
 
 param set-default CA_AIRFRAME 1
 
+param set-default COM_PREARM_MODE 2
+param set-default CBRK_AIRSPD_CHK 162128
+
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
 

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

@@ -14,9 +14,14 @@ param set-default MAV_TYPE 1
 # Default parameters for fixed wing UAVs.
 #
 param set-default COM_POS_FS_DELAY 5
-param set-default COM_POS_FS_EPH 15
+
+# there is a 2.5 factor applied on the _FS thresholds if for invalidation
+param set-default COM_POS_FS_EPH 50
 param set-default COM_POS_FS_EPV 30
 param set-default COM_VEL_FS_EVH 5
+
+param set-default COM_POS_LOW_EPH 50
+
 # Disable preflight disarm to not interfere with external launching
 param set-default COM_DISARM_PRFLT -1
 

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

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -10,6 +10,11 @@ set VEHICLE_TYPE vtol
 # MAV_TYPE_VTOL_FIXEDROTOR 22
 param set-default MAV_TYPE 22
 
+# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
+param set-default COM_POS_FS_EPH 50
+
+param set-default COM_POS_LOW_EPH 50
+
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_YAW_TMT 10
 

Tools/setup/ubuntu.sh

@@ -217,6 +217,17 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
 
+	# Install Gazebo
+	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+		# Update list, since new gazebo-stable.list has been added
+		sudo apt-get update -y --quiet
+		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+			ignition-fortress \
+			;
+	fi
+
 	# Install Gazebo classic
 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 		gazebo_version=9
@@ -255,16 +266,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		echo "export SVGA_VGPU10=0" >> ~/.profile
 	fi
 
-	# Install Gazebo
-	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-		# Update list, since new gazebo-stable.list has been added
-		sudo apt-get update -y --quiet
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			ignition-fortress \
-			;
-	fi
 fi
 
 if [[ $INSTALL_NUTTX == "true" ]]; then

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>

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

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

@@ -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-h7/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/pixracerpro/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/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/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/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-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

@@ -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/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/FailsafeFlags.msg

@@ -48,6 +48,7 @@ bool mission_failure                  # Mission failure
 bool vtol_fixed_wing_system_failure   # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
 bool wind_limit_exceeded              # Wind limit exceeded
 bool flight_time_limit_exceeded       # Maximum flight time exceeded
+bool local_position_accuracy_low      # Local position estimate has dropped below threshold, but is currently still declared valid
 
 # Failure detector
 bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)

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/common/include/px4_platform_common/tasks.h

@@ -141,8 +141,12 @@ typedef struct {
 // The navigation system needs to execute regularly but has no realtime needs
 #define SCHED_PRIORITY_NAVIGATION		(SCHED_PRIORITY_DEFAULT + 5)
 //      SCHED_PRIORITY_DEFAULT
-#define SCHED_PRIORITY_LOG_WRITER		(SCHED_PRIORITY_DEFAULT - 10)
+
 #define SCHED_PRIORITY_PARAMS			(SCHED_PRIORITY_DEFAULT - 15)
+
+#define SCHED_PRIORITY_LOG_WRITER		(SCHED_PRIORITY_DEFAULT - 40)
+
+
 //      SCHED_PRIORITY_IDLE
 
 typedef int (*px4_main_t)(int argc, char *argv[]);

platforms/nuttx/src/px4/common/px4_mtd.cpp

@@ -308,7 +308,7 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd)
 
 		instances[i] = new mtd_instance_s;
 
-		if (instances == nullptr) {
+		if (instances[i] == nullptr) {
 memoryout:
 			PX4_ERR("failed to allocate memory!");
 			return rv;
@@ -444,41 +444,39 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get)
 {
 	int rv = -ENODEV;
 
-	if (instances != nullptr) {
+	static const char *keys[] = PX4_MFT_MTD_STR_TYPES;
+	static const px4_mtd_types_t types[] = PX4_MFT_MTD_TYPES;
+	int key = 0;
 
-		static const char *keys[] = PX4_MFT_MTD_STR_TYPES;
-		static const px4_mtd_types_t types[] = PX4_MFT_MTD_TYPES;
-		int key = 0;
-
-		for (unsigned int k = 0; k < arraySize(keys); k++) {
-			if (!strcmp(keys[k], sub)) {
-				key = types[k];
-				break;
-			}
+	for (unsigned int k = 0; k < arraySize(keys); k++) {
+		if (!strcmp(keys[k], sub)) {
+			key = types[k];
+			break;
 		}
+	}
 
 
-		rv = -EINVAL;
+	rv = -EINVAL;
 
-		if (key != 0) {
-			rv = -ENOENT;
+	if (key != 0) {
+		rv = -ENOENT;
 
-			for (int i = 0; i < num_instances; i++) {
-				for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
-					if (instances[i]->partition_types[n] == key) {
-						if (get != nullptr && val == nullptr) {
-							*get =  instances[i]->partition_names[n];
-							return 0;
-						}
+		for (int i = 0; i < num_instances; i++) {
+			for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
+				if (instances[i]->partition_types[n] == key) {
+					if (get != nullptr && val == nullptr) {
+						*get =  instances[i]->partition_names[n];
+						return 0;
+					}
 
-						if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) {
-							return 0;
-						}
+					if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) {
+						return 0;
 					}
 				}
 			}
 		}
 	}
 
+
 	return rv;
 }

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/drv_sensor.h

@@ -111,11 +111,11 @@
 #define DRV_DIFF_PRESS_DEVTYPE_SDP32     0x4B
 #define DRV_DIFF_PRESS_DEVTYPE_SDP33     0x4C
 
-#define DRV_BARO_DEVTYPE_LPS33HW        0x4C
 #define DRV_BARO_DEVTYPE_TCBP001TA      0x4D
 #define DRV_BARO_DEVTYPE_MS5837         0x4E
 #define DRV_BARO_DEVTYPE_SPL06          0x4F
 
+#define DRV_BARO_DEVTYPE_LPS33HW        0x50
 #define DRV_BARO_DEVTYPE_MPL3115A2	0x51
 #define DRV_ACC_DEVTYPE_FXOS8701C	0x52
 

src/drivers/qshell/posix/qshell.cpp

@@ -47,9 +47,19 @@
 // Static variables
 px4::AppState QShell::appState;
 uint32_t QShell::_current_sequence{0};
+uORB::Subscription *QShell::_qshell_retval_sub{nullptr};
+
 
 int QShell::main(std::vector<std::string> argList)
 {
+	if (_qshell_retval_sub == nullptr) {
+		_qshell_retval_sub = new uORB::Subscription(ORB_ID(qshell_retval));
+
+		if (_qshell_retval_sub == nullptr) {
+			PX4_ERR("Couldn't initialilze Qshell response subscription");
+		}
+	}
+
 	int ret = _send_cmd(argList);
 
 	if (ret != 0) {
@@ -109,7 +119,7 @@ int QShell::_wait_for_retval()
 	memset(&retval, 0, sizeof(qshell_retval_s));
 
 	while (hrt_elapsed_time(&time_started_us) < QSHELL_RESPONSE_WAIT_TIME_US) {
-		if (_qshell_retval_sub.update(&retval)) {
+		if (_qshell_retval_sub->update(&retval)) {
 			if (retval.return_sequence != _current_sequence) {
 				PX4_WARN("Ignoring return value with wrong sequence");
 

src/drivers/qshell/posix/qshell.h

@@ -58,7 +58,7 @@ private:
 
 	uORB::Publication<qshell_req_s>	_qshell_req_pub{ORB_ID(qshell_req)};
 
-	uORB::Subscription		_qshell_retval_sub{ORB_ID(qshell_retval)};
+	static uORB::Subscription	*_qshell_retval_sub;
 
 	static uint32_t			_current_sequence;
 };

src/lib/CMakeLists.txt

@@ -53,7 +53,6 @@ add_subdirectory(l1 EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
-add_subdirectory(mission EXCLUDE_FROM_ALL)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)

src/lib/mission/CMakeLists.txt

@@ -1,36 +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.
-#
-############################################################################
-
-px4_add_library(planned_mission_interface
-	planned_mission_interface.cpp
-)

src/lib/mission/planned_mission_interface.cpp

@@ -1,595 +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.
- *
- ****************************************************************************/
-
-/**
- * @file planned_mission_interface.cpp
- *
- */
-
-#include "planned_mission_interface.h"
-
-#include <stdlib.h>
-
-#include "dataman/dataman.h"
-#include "lib/geo/geo.h"
-#include "modules/navigator/mission_block.h"
-
-void PlannedMissionInterface::update()
-{
-	_home_pos_sub.update();
-
-	if (!_is_land_start_item_searched && _home_pos_sub.get().valid_alt) {
-		findLandStartItem();
-		_is_land_start_item_searched = true;
-	}
-
-	if (_mission_sub.updated()) {
-		mission_s new_mission;
-		_mission_sub.update(&new_mission);
-
-		if (isMissionValid(new_mission)) {
-			/* Check if it was updated externally*/
-			if (new_mission.timestamp > _mission.timestamp) {
-				bool mission_waypoints_changed{checkMissionWaypointsChanged(_mission, new_mission)};
-				_mission = new_mission;
-
-				if (goToItem(new_mission.current_seq, true) == EXIT_SUCCESS) {
-					if (_home_pos_sub.get().valid_alt) {
-						findLandStartItem();
-
-					} else {
-						_is_land_start_item_searched = false;
-					}
-
-					onMissionUpdate(mission_waypoints_changed);
-				}
-			}
-		}
-	}
-}
-
-void PlannedMissionInterface::getPreviousPositionItems(int32_t start_index, struct mission_item_s items[],
-		size_t &num_found_items, uint8_t max_num_items) const
-{
-	num_found_items = 0u;
-
-	int32_t next_mission_index{start_index};
-
-	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index < 0) {
-			break;
-		}
-
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
-
-		do {
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == EXIT_SUCCESS;
-			next_mission_index--;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items[item_idx] = next_mission_item;
-			num_found_items = item_idx + 1;
-
-		} else {
-			break;
-		}
-	}
-}
-
-void PlannedMissionInterface::getNextPositionItems(int32_t start_index, struct mission_item_s items[],
-		size_t &num_found_items, uint8_t max_num_items) const
-{
-	// Make sure vector does not contain any preexisting elements.
-	num_found_items = 0u;
-
-	int32_t next_mission_index{start_index};
-
-	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index >= _mission.count) {
-			break;
-		}
-
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
-
-		do {
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == EXIT_SUCCESS;
-			next_mission_index++;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items[item_idx] = next_mission_item;
-			num_found_items = item_idx + 1;
-
-		} else {
-			break;
-		}
-	}
-}
-
-int PlannedMissionInterface::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump,
-		bool write_jumps, bool mission_direction_backward) const
-{
-	if (mission_index >= _mission.count || mission_index < 0) {
-		return EXIT_FAILURE;
-	}
-
-	int32_t new_mission_index{mission_index};
-	mission_item_s new_mission;
-
-	for (uint16_t jump_count = 0u; jump_count < _max_jump_iteraion; jump_count++) {
-		if (readMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not read next item.");
-			return EXIT_FAILURE;
-		}
-
-		if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) {
-			if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) {
-				PX4_ERR("Do Jump mission index is out of bounds.");
-				return EXIT_FAILURE;
-			}
-
-			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) {
-				if (write_jumps) {
-					new_mission.do_jump_current_count++;
-
-					if (writeMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
-						PX4_ERR("Could not update jump count on mission item.");
-						// Still continue searching for next non jump item.
-					}
-				}
-
-				new_mission_index = new_mission.do_jump_mission_index;
-
-			} else {
-				if (mission_direction_backward) {
-					new_mission_index--;
-
-				} else {
-					new_mission_index++;
-				}
-			}
-
-		} else {
-			break;
-		}
-	}
-
-	mission_index = new_mission_index;
-	mission = new_mission;
-
-	return EXIT_SUCCESS;
-}
-
-bool PlannedMissionInterface::hasMissionLandStart() const
-{
-	return (_land_start_index != _invalid_index) && (_land_start_index < _mission.count);
-}
-
-int PlannedMissionInterface::goToNextItem(bool execute_jump)
-{
-	if (_mission.current_seq + 1 >= (_mission.count)) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_mission.current_seq + 1, execute_jump);
-}
-
-int PlannedMissionInterface::goToPreviousItem(bool execute_jump)
-{
-	if (_mission.current_seq <= 0) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_mission.current_seq - 1, execute_jump, true);
-}
-
-int PlannedMissionInterface::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward)
-{
-	mission_item_s mission_item;
-
-	if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == EXIT_SUCCESS) {
-		if (setMissionIndex(index) == EXIT_SUCCESS) {
-			_current_planned_mission_item = mission_item;
-
-		} else {
-			return EXIT_FAILURE;
-		}
-
-	} else {
-		return EXIT_FAILURE;
-	}
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::goToPreviousPositionItem(bool execute_jump)
-{
-	do {
-		if (goToPreviousItem(execute_jump) != EXIT_SUCCESS) {
-			return EXIT_FAILURE;
-		}
-	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::goToNextPositionItem(bool execute_jump)
-{
-	do {
-		if (goToNextItem(execute_jump) != EXIT_SUCCESS) {
-			return EXIT_FAILURE;
-		}
-	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::setMissionIndex(int32_t index)
-{
-	// Nothing to do if it is already at the current index.
-	if (index == _mission.current_seq) {
-		return EXIT_SUCCESS;
-	}
-
-	mission_s mission{_mission};
-	mission.current_seq = index;
-	mission.timestamp = hrt_absolute_time();
-
-	if (writeMission(mission) == EXIT_SUCCESS) {
-		_mission = mission;
-		return EXIT_SUCCESS;
-
-	} else {
-		return EXIT_FAILURE;
-	}
-}
-
-int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt,
-		const vehicle_status_s &vehicle_status)
-{
-	int32_t min_dist_index(-1);
-	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
-
-	for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, mission_item_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not set mission closest to position.");
-			return EXIT_FAILURE;
-		}
-
-		if (MissionBlock::item_contains_position(mission)) {
-			// do not consider land waypoints for a fw
-			if (!((mission.nav_cmd == NAV_CMD_LAND) &&
-			      (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
-			      (!vehicle_status.is_vtol))) {
-				float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon,
-						MissionBlock::get_absolute_altitude_for_item(mission, home_alt),
-						lat,
-						lon,
-						alt,
-						&dist_xy, &dist_z);
-
-				if (dist < min_dist) {
-					min_dist = dist;
-					min_dist_index = mission_item_index;
-				}
-			}
-		}
-	}
-
-	return goToItem(min_dist_index, false);
-}
-
-int PlannedMissionInterface::goToMissionLandStart()
-{
-	if (!hasMissionLandStart()) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_land_start_index, false);
-}
-
-int PlannedMissionInterface::initMission()
-{
-	mission_s mission;
-	bool ret_val{EXIT_FAILURE};
-
-	if (readMission(mission) == EXIT_SUCCESS) {
-		_mission = mission;
-
-		if (goToItem(mission.current_seq, true) == EXIT_SUCCESS) {
-			findLandStartItem();
-			ret_val = EXIT_SUCCESS;
-		}
-
-	} else {
-		resetMission();
-	}
-
-	_mission_pub.advertise();
-
-	return ret_val;
-}
-
-void PlannedMissionInterface::resetMission()
-{
-	/* we do not need to reset mission if there is none.*/
-	if (_mission.count == 0u) {
-		return;
-	}
-
-	/* Set a new mission*/
-	mission_s new_mission{.timestamp = hrt_absolute_time(),
-			      .current_seq = 0,
-			      .count = 0u,
-			      .dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0};
-
-	if (writeMission(new_mission) == EXIT_SUCCESS) {
-		_mission = new_mission;
-
-	} else {
-		PX4_ERR("Mission Initialization failed.");
-	}
-}
-
-void PlannedMissionInterface::resetMissionJumpCounter()
-{
-	for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) {
-		mission_item_s mission_item;
-
-		if (readMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not read mission item for jump count reset.");
-			break;
-		}
-
-		if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) {
-			mission_item.do_jump_current_count = 0u;
-
-			if (writeMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
-				PX4_ERR("Could not write mission item for jump count reset.");
-				break;
-			}
-		}
-	}
-}
-
-int PlannedMissionInterface::writeMission(mission_s &mission)
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (!isMissionValid(mission)) {
-		return EXIT_FAILURE;
-	}
-
-	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
-		return EXIT_FAILURE;
-	}
-
-	if (dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
-		PX4_ERR("Can't save mission state");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		_mission_pub.publish(mission);
-	}
-
-	dm_unlock(DM_KEY_MISSION_STATE);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::readMission(mission_s &read_mission) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}
-
-	mission_s mission;
-
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
-		PX4_ERR("Can't read mission state.");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		if (isMissionValid(mission)) {
-			read_mission = mission;
-
-		} else {
-			ret_val = EXIT_FAILURE;
-		}
-	}
-
-	dm_unlock(DM_KEY_MISSION_STATE);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::readMissionItem(mission_item_s &read_mission_item, size_t index) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (index >= _mission.count) {
-		return EXIT_FAILURE;
-	}
-
-	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
-
-	/* lock current mission item */
-	/*int dm_lock_ret = dm_lock(current_dm_item);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}*/
-
-	mission_item_s mission_item;
-
-	if (dm_read(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
-		PX4_ERR("Can't read mission item from DM.");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		read_mission_item = mission_item;
-	}
-
-	//dm_unlock(current_dm_item);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::writeMissionItem(const mission_item_s &mission_item, size_t index) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (index >= _mission.count) {
-		return EXIT_FAILURE;
-	}
-
-	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
-
-	/* lock current mission item */
-	/*int dm_lock_ret = dm_lock(current_dm_item);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}*/
-
-	if (dm_write(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
-		PX4_ERR("Can't write mission item to DM.");
-		ret_val = EXIT_FAILURE;
-	}
-
-	//dm_unlock(current_dm_item);
-
-	return ret_val;
-}
-
-bool PlannedMissionInterface::isMissionValid(mission_s &mission) const
-{
-	bool ret_val{false};
-
-	if ((mission.current_seq < mission.count) &&
-	    (mission.current_seq >= 0) &&
-	    (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
-	    (mission.timestamp != 0u)) {
-		ret_val = true;
-
-	}
-
-	return ret_val;
-}
-
-void PlannedMissionInterface::findLandStartItem()
-{
-	_land_start_index = _invalid_index;
-	_land_index = _invalid_index;
-
-	for (size_t mission_item_index = 1u; mission_item_index < _mission.count; mission_item_index++) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, mission_item_index) == EXIT_SUCCESS) {
-			if (mission.nav_cmd == NAV_CMD_DO_LAND_START) {
-				_land_start_index = mission_item_index;
-			}
-
-			if ((mission.nav_cmd == NAV_CMD_VTOL_LAND) || (mission.nav_cmd == NAV_CMD_LAND)) {
-				_land_index = mission_item_index;
-				break;
-			}
-
-		} else {
-			break;
-		}
-	}
-
-	if (_land_start_index != _invalid_index) {
-		mission_item_s mission;
-		size_t num_found_item{0u};
-		getNextPositionItems(_land_start_index, &mission, num_found_item, 1u);
-
-		if (num_found_item == 1u) {
-			_land_start_pos.lat = mission.lat;
-			_land_start_pos.lon = mission.lon;
-			_land_start_pos.alt = mission.altitude_is_relative ?	mission.altitude +
-					      _home_pos_sub.get().alt : mission.altitude;
-
-		} else {
-			PX4_ERR("Could not read land start coordinates.");
-			_land_start_pos.lat = 0.0;
-			_land_start_pos.lon = 0.0;
-		}
-
-		_land_pos.lat = _land_start_pos.lat;
-		_land_pos.lon = _land_start_pos.lon;
-		_land_pos.alt = _land_start_pos.alt;
-	}
-
-	if (_land_index != _invalid_index) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, _land_index) == EXIT_SUCCESS) {
-			_land_pos.lat = mission.lat;
-			_land_pos.lon = mission.lon;
-			_land_pos.alt = mission.altitude_is_relative ?	mission.altitude +
-					_home_pos_sub.get().alt : mission.altitude;
-		}
-
-		if (_land_start_index == _invalid_index) {
-			_land_start_index = _land_index;
-			_land_start_pos.lat = _land_pos.lat;
-			_land_start_pos.lon = _land_pos.lon;
-			_land_start_pos.alt = _land_pos.alt;
-		}
-	}
-}
-
-bool PlannedMissionInterface::checkMissionWaypointsChanged(const mission_s &old_mission,
-		const mission_s &new_mission) const
-{
-	return (new_mission.count != old_mission.count) || (new_mission.dataman_id != old_mission.dataman_id);
-}

src/lib/mission/planned_mission_interface.h

@@ -1,111 +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.
- *
- ****************************************************************************/
-
-/**
- * @file planned_mission_interface.h
- *
- */
-
-#pragma once
-
-#include <stdint.h>
-
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/uORB.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include "navigator/navigation.h"
-
-class PlannedMissionInterface
-{
-public:
-	void update();
-	void getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
-				      uint8_t max_num_items) const;
-	void getNextPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
-				  uint8_t max_num_items) const;
-	bool hasMissionLandStart() const;
-	int goToNextItem(bool execute_jump);
-	int goToPreviousItem(bool execute_jump);
-	int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
-	int goToPreviousPositionItem(bool execute_jump);
-	int goToNextPositionItem(bool execute_jump);
-	int goToMissionLandStart();
-	int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status);
-	virtual void onMissionUpdate(bool has_mission_items_changed) = 0;
-
-	int initMission();
-	void resetMission();
-	void resetMissionJumpCounter();
-
-private:
-	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
-			   bool mission_direction_backward = false) const;
-	int setMissionIndex(int32_t index);
-	int writeMission(mission_s &mission);
-	int readMission(mission_s &read_mission) const;
-	int readMissionItem(mission_item_s &read_mission_item, size_t index) const;
-	int writeMissionItem(const mission_item_s &mission_item, size_t index) const ;
-	bool isMissionValid(mission_s &mission) const;
-	void findLandStartItem();
-	bool checkMissionWaypointsChanged(const mission_s &old_mission, const mission_s &new_mission) const;
-public:
-	static const uint16_t _invalid_index{UINT16_MAX};
-private:
-	static const uint16_t _max_jump_iteraion{10u};
-protected:
-	mission_s _mission;
-	mission_item_s _current_planned_mission_item;
-	uint16_t _land_start_index;
-	uint16_t _land_index;
-	struct {
-		double lat;
-		double lon;
-		float alt;
-	} _land_start_pos{.lat = 0.0,
-				  .lon = 0.0,
-				  .alt = 0.0f},
-			       _land_pos{.lat = 0.0,
-					 .lon = 0.0,
-					 .alt = 0.0f};
-
-private:
-	bool _is_land_start_item_searched{false};
-
-	uORB::Subscription _mission_sub{ORB_ID(mission)};
-	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
-	uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
-};