[WIP] logger priority boost command line

EKF2_MAG_TYPE - fix typos (#20808 )
parameter_translation: Add parameter translation for renamed L1 parameters
2026-06-01 11:20:06 +08:00 · 2023-02-10 19:26:14 -05:00 · 2023-02-10 14:39:57 +01:00 · 2023-02-09 17:51:55 +01:00 · 2023-02-09 17:51:55 +01:00 · 2023-02-09 17:51:55 +01:00
184 changed files with 2620 additions and 1876 deletions
@@ -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",
@@ -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
@@ -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"
        }
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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

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

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

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

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

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

@@ -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
@@ -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>
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -126,6 +126,7 @@ set(msg_files
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
+	ModeCompleted.msg
 	NavigatorMissionItem.msg
 	NpfgStatus.msg
 	ObstacleDistance.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
@@ -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)
@@ -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
@@ -15,6 +15,7 @@ uint32 mode_req_global_position
 uint32 mode_req_mission
 uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
+uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
 uint32 mode_req_other             # other requirements, not covered above (for external modes)

@@ -47,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)
@@ -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
+
@@ -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)
 /****************************************************************************
@@ -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[]);
@@ -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;
 }
@@ -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)
@@ -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
@@ -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
@@ -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,
@@ -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

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

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

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

@@ -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;
 };
@@ -1254,11 +1254,9 @@ UavcanNode::get_next_dirty_node_id(uint8_t base)
 */
 static void print_usage()
 {
-	PX4_INFO_RAW("usage: \n"
-		     "\tuavcan {start|status|stop|shrink|update}\n"
-		     "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>"
-		     "\t        reset <node-id>"
-		    );
+	PX4_INFO("usage: \n"
+		 "\tuavcan {start|status|stop|shrink|update}\n"
+		 "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>");
 }

 extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
@@ -1319,17 +1317,6 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}

-	if (!std::strcmp(argv[1], "reset")) {
-		int node_arg = 2;
-
-		if (argc < node_arg + 1) {
-			errx(1, "Node id required");
-		}
-
-		int nodeid = atoi(argv[node_arg]);
-		return inst->reset_node(nodeid);
-	}
-
 	/*
 	 * Parameter setting commands
 	 *
@@ -1339,9 +1326,9 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 	 *  uavcan param set <node> <name> <value>
 	 *
 	 */
-	if (!std::strcmp(argv[1], "param")) {
-		int node_arg = 3;
+	int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;

+	if (!std::strcmp(argv[1], "param") || node_arg == 2) {
 		if (argc < node_arg + 1) {
 			errx(1, "Node id required");
 		}
@@ -1352,7 +1339,11 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 			errx(1, "Invalid Node id");
 		}

-		if (!std::strcmp(argv[2], "list")) {
+		if (node_arg == 2) {
+
+			return inst->reset_node(nodeid);
+
+		} else if (!std::strcmp(argv[2], "list")) {

 			return inst->list_params(nodeid);

@@ -47,8 +47,9 @@
 using matrix::Vector2d;
 using matrix::Vector2f;

-void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &unit_path_tangent,
-		       const float signed_track_error, const float path_curvature)
+void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel,
+		       const Vector2f &unit_path_tangent,
+		       const Vector2f &position_on_path, const float path_curvature)
 {
 	const float ground_speed = ground_vel.norm();

@@ -57,6 +58,9 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con

 	const float wind_speed = wind_vel.norm();

+	const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
+	const float signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle);
+
 	// on-track wind triangle projections
 	const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
 	const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
@@ -115,62 +119,10 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
 	// ramped in as the vehicle approaches the track and is further smoothly
 	// zeroed out as the bearing becomes infeasible.
 	lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
+
+	updateRollSetpoint();
 } // guideToPath

-void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &bearing_vec,
-			const float track_error)
-{
-	bearing_vec_ = bearing_vec; // for status output
-
-	const float ground_speed = ground_vel.norm();
-
-	const Vector2f air_vel = ground_vel - wind_vel;
-	const float airspeed = air_vel.norm();
-
-	const float wind_speed = wind_vel.norm();
-
-	// wind triangle projections
-	const float wind_cross_bearing = wind_vel.cross(bearing_vec);
-	const float wind_dot_bearing = wind_vel.dot(bearing_vec);
-
-	// continuous representation of the bearing feasibility
-	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
-	feas_on_track_ = feas_; // no distinction in point following - set only for recording
-
-	// update control parameters considering upper and lower stability bounds (if enabled)
-	// must be called before trackErrorBound() as it updates time_const_
-	// NOTE: track error input as 0 for the period adaptation as track proximity will
-	//       only ramp in 1) curvature based lower bounding, of which there is none
-	//       for a point, and 2) period upper bounds, which for zero curvature is
-	//       infinite, and thus disregarded in this case.
-	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, 0.0f, 0.0f,
-				      wind_vel, bearing_vec, feas_);
-	p_gain_ = pGain(adapted_period_, damping_);
-	time_const_ = timeConst(adapted_period_, damping_);
-
-	// track error bound is dynamic depending on ground speed
-	track_error_bound_ = trackErrorBound(ground_speed, time_const_);
-	const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
-
-	// look ahead angle based solely on track proximity
-	const float look_ahead_ang = lookAheadAngle(normalized_track_error);
-
-	track_proximity_ = trackProximity(look_ahead_ang);
-
-	min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_);
-
-	// reference air velocity with directional feedforward effect for following
-	// curvature in wind and magnitude incrementation depending on minimum ground
-	// speed violations and/or high wind conditions in general
-	air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec, wind_cross_bearing,
-				      wind_dot_bearing, wind_speed, min_ground_speed_ref_);
-	airspeed_ref_ = air_vel_ref_.norm();
-
-	// lateral acceleration demand based on heading error
-	lateral_accel_ff_ = 0.0f;
-	lateral_accel_ = lateralAccel(air_vel, air_vel_ref_, airspeed);
-} // guideToPoint
-
 float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
 			const float track_error, const float path_curvature, const Vector2f &wind_vel,
 			const Vector2f &unit_path_tangent, const float feas_on_track) const
@@ -309,7 +261,7 @@ float NPFG::timeConst(const float period, const float damping) const

 float NPFG::lookAheadAngle(const float normalized_track_error) const
 {
-	return M_PI_F * 0.5f * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
+	return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
 } // lookAheadAngle

 Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
@@ -514,182 +466,6 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
 	}
 } // lateralAccel

-/*******************************************************************************
- * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
- */
-
-void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B,
-			     const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
-{
-	// similar to logic found in ECL_L1_Pos_Controller method of same name
-	// BUT no arbitrary max approach angle, approach entirely determined by generated
-	// bearing vectors
-
-	path_type_loiter_ = false;
-
-	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
-	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
-
-	if (vector_A_to_B.norm() < NPFG_EPSILON) {
-		// the waypoints are on top of each other and should be considered as a
-		// single waypoint, fly directly to it
-		unit_path_tangent_ = -vector_A_to_vehicle.normalized();
-		signed_track_error_ = vector_A_to_vehicle.norm();
-		closest_point_on_path_ = waypoint_A;
-		guideToPoint(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_);
-
-	} else if (vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) {
-		// we are in front of waypoint A, fly directly to it until the bearing generated
-		// to the line segement between A and B is shallower than that from the
-		// bearing to the first waypoint (A).
-
-		// guidance to the line through A and B
-		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
-		closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_;
-		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
-
-		const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
-
-		if (unit_path_tangent_.dot(bearing_vec_) < unit_path_tangent_.dot(bearing_vec_to_point)) {
-			// we are in front of the first waypoint and the bearing to the point is
-			// shallower than that to the line. reset path params to fly directly to
-			// the first waypoint.
-
-			// TODO: probably better to blend these instead of hard switching (could
-			// affect the adaptive tuning if we switch between these cases with wind
-			// gusts)
-
-			unit_path_tangent_ = bearing_vec_to_point;
-			signed_track_error_ = vector_A_to_vehicle.norm();
-			closest_point_on_path_ = waypoint_A;
-			guideToPoint(ground_vel, wind_vel, bearing_vec_to_point, signed_track_error_);
-		}
-
-	} else {
-		// track the line segment between A and B
-		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
-		closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_;
-		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
-	}
-
-	updateRollSetpoint();
-} // navigateWaypoints
-
-void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
-			  float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
-{
-	path_type_loiter_ = true;
-
-	radius = math::max(radius, MIN_RADIUS);
-
-	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
-
-	Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
-	const float dist_to_center = vector_center_to_vehicle.norm();
-
-	// find the direction from the circle center to the closest point on its perimeter
-	// from the vehicle position
-	Vector2f unit_vec_center_to_closest_pt;
-
-	if (dist_to_center < 0.1f) {
-		// the logic breaks down at the circle center, employ some mitigation strategies
-		// until we exit this region
-		if (ground_vel.norm() < 0.1f) {
-			// arbitrarily set the point in the northern top of the circle
-			unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
-
-		} else {
-			// set the point in the direction we are moving
-			unit_vec_center_to_closest_pt = ground_vel.normalized();
-		}
-
-	} else {
-		// set the point in the direction of the aircraft
-		unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
-	}
-
-	// 90 deg clockwise rotation * loiter direction
-	unit_path_tangent_ = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
-
-	// positive in direction of path normal
-	signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius);
-
-	closest_point_on_path_ = unit_vec_center_to_closest_pt * radius + loiter_center;
-
-	float path_curvature = loiter_direction_multiplier / radius;
-
-	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
-
-	updateRollSetpoint();
-} // navigateLoiter
-
-
-void NPFG::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
-			       const matrix::Vector2f &tangent_setpoint,
-			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
-{
-	path_type_loiter_ = false;
-
-	// set unit tangent directly
-	unit_path_tangent_ = tangent_setpoint.normalized();
-
-	// closest point to vehicle
-	matrix::Vector2f error_vector = vehicle_pos - position_setpoint;
-	closest_point_on_path_ = position_setpoint;
-	signed_track_error_ = unit_path_tangent_.cross(error_vector);
-
-	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
-
-	updateRollSetpoint();
-} // navigatePathTangent
-
-void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
-{
-	path_type_loiter_ = false;
-
-	Vector2f air_vel = ground_vel - wind_vel;
-	unit_path_tangent_ = Vector2f{cosf(heading_ref), sinf(heading_ref)};
-	signed_track_error_ = 0.0f;
-
-	closest_point_on_path_.setNaN();
-
-	// use the guidance law to regulate heading error - ignoring wind or inertial position
-	guideToPath(air_vel, Vector2f{0.0f, 0.0f}, unit_path_tangent_, signed_track_error_, 0.0f);
-
-	updateRollSetpoint();
-} // navigateHeading
-
-void NPFG::navigateBearing(float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel)
-{
-	path_type_loiter_ = false;
-
-	unit_path_tangent_ = Vector2f{cosf(bearing), sinf(bearing)};
-
-	signed_track_error_ = 0.0f;
-
-	// no track error or path curvature to consider, just regulate ground velocity
-	// to bearing vector
-	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
-
-	updateRollSetpoint();
-} // navigateBearing
-
-void NPFG::navigateLevelFlight(const float heading)
-{
-	path_type_loiter_ = false;
-
-	airspeed_ref_ = airspeed_nom_;
-	lateral_accel_ = 0.0f;
-	feas_ = 1.0f;
-	bearing_vec_ = Vector2f{cosf(heading), sinf(heading)};
-	unit_path_tangent_ = bearing_vec_;
-	signed_track_error_ = 0.0f;
-
-	updateRollSetpoint();
-} // navigateLevelFlight
-
 float NPFG::switchDistance(float wp_radius) const
 {
 	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
@@ -71,6 +71,22 @@ class NPFG
 {

 public:
+	/*
+	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * a path in wind (including excess wind conditions).
+	 *
+	 * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] position_on_path Horizontal point on the path to track described in local coordinates [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 */
+	void guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel,
+			 const matrix::Vector2f &wind_vel,
+			 const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path,
+			 const float path_curvature);

 	/*
 	 * Set the nominal controller period [s].
@@ -180,8 +196,6 @@ public:
 	 */
 	float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); }

-	matrix::Vector2f getClosestPoint() const { return closest_point_on_path_;}
-
 	/*
 	 * @return Bearing angle [rad]
 	 */
@@ -213,91 +227,6 @@ public:
 	 */
 	float getMinGroundSpeedRef() const { return min_ground_speed_ref_; }

-	/*******************************************************************************
-	 * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
-	 */
-
-	/*
-	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
-	 * method of the same name. Takes two waypoints and determines the relevant
-	 * parameters for evaluating the NPFG guidance law, then updates control setpoints.
-	 *
-	 * @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates
-	 *            (lat,lon) [deg]
-	 * @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates
-	 *            (lat,lon) [deg]
-	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 */
-	void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B,
-			       const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
-			       const matrix::Vector2f &wind_vel);
-
-	/*
-	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
-	 * determines the relevant parameters for evaluating the NPFG guidance law,
-	 * then updates control setpoints.
-	 *
-	 * @param[in] loiter_center The position of the center of the loiter circle [m]
-	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] radius Loiter radius [m]
-	 * @param[in] loiter_direction_counter_clockwise Specifies loiter direction
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 */
-	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
-			    float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
-			    const matrix::Vector2f &wind_vel);
-
-	/*
-	 * Path following logic. Takes poisiton, path tangent, curvature and
-	 * then updates control setpoints to follow a path setpoint.
-	 *
-	 * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] curvature of the path setpoint [1/m]
-	 */
-	void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
-				 const matrix::Vector2f &tangent_setpoint,
-				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
-
-	/*
-	 * Navigate on a fixed heading.
-	 *
-	 * This only holds a certain (air mass relative) direction and does not perform
-	 * cross track correction. Helpful for semi-autonomous modes. Introduced
-	 * by in ECL_L1_Pos_Controller, augmented for use with NPFG here.
-	 *
-	 * @param[in] heading_ref Reference heading angle [rad]
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 */
-	void navigateHeading(float heading_ref, const matrix::Vector2f &ground_vel,
-			     const matrix::Vector2f &wind_vel);
-
-	/*
-	 * Navigate on a fixed bearing.
-	 *
-	 * This only holds a certain (ground relative) direction and does not perform
-	 * cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading.
-	 *
-	 * @param[in] bearing Bearing angle [rad]
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 */
-	void navigateBearing(float bearing, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
-
-	/*
-	 * Keep the wings level.
-	 *
-	 * @param[in] heading Heading angle [rad]
-	 */
-	void navigateLevelFlight(const float heading);
-
 	/*
 	 * [Copied directly from ECL_L1_Pos_Controller]
 	 *
@@ -331,25 +260,6 @@ public:
 	 */
 	float switchDistance(float wp_radius) const;

-	/*
-	 * The path bearing
-	 *
-	 * @return bearing angle (-pi..pi, in NED frame) [rad]
-	 */
-	float targetBearing() const { return atan2f(unit_path_tangent_(1), unit_path_tangent_(0)); }
-
-	/*
-	 * [Copied directly from ECL_L1_Pos_Controller]
-	 *
-	 * Returns true if the loiter waypoint has been reached
-	 */
-	bool reachedLoiterTarget() { return circleMode(); }
-
-	/*
-	 * Returns true if following a circle (loiter)
-	 */
-	bool circleMode() { return path_type_loiter_ && track_proximity_ > NPFG_EPSILON; }
-
 	/*
 	 * [Copied directly from ECL_L1_Pos_Controller]
 	 *
@@ -421,10 +331,8 @@ private:
 	float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track

 	// path following states
-	matrix::Vector2f unit_path_tangent_{matrix::Vector2f{1.0f, 0.0f}}; // unit path tangent vector
 	float signed_track_error_{0.0f}; // signed track error [m]
 	matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector
-	matrix::Vector2f closest_point_on_path_{matrix::Vector2f{NAN, NAN}}; // instantaneous position setpoint [m]

 	/*
 	 * guidance outputs
@@ -443,35 +351,6 @@ private:
 	float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
 	float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
 	float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s]
-	bool path_type_loiter_{false}; // true if the guidance law is tracking a loiter circle
-
-	/*
-	 * Computes the lateral acceleration and airspeed references necessary to track
-	 * a path in wind (including excess wind conditions).
-	 *
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
-	 *            in direction of path
-	 * @param[in] signed_track_error Signed error to track at closest point (sign
-	 *            determined by path normal direction) [m]
-	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
-	 */
-	void guideToPath(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
-			 const matrix::Vector2f &unit_path_tangent, const float signed_track_error,
-			 const float path_curvature);
-
-	/*
-	 * Computes the lateral acceleration and airspeed references necessary to track
-	 * a point in wind (including excess wind conditions).
-	 *
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] bearing_vec Unit vector from vehicle to the target point
-	 * @param[in] track_error Distance from vehicle to the target point [m]
-	 */
-	void guideToPoint(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
-			  const matrix::Vector2f &bearing_vec, const float track_error);

 	/*
 	 * Adapts the controller period considering user defined inputs, current flight
@@ -211,4 +211,25 @@ bool param_modify_on_import(bson_node_t node)
 	}

 	return false;
+
+	//2023-02-08: translate L1 parameters after removing l1 control
+	{
+		if (strcmp("RWTO_L1_PERIOD", node->name) == 0) {
+			strcpy(node->name, "RWTO_NPFG_PERIOD");
+			PX4_INFO("copying %s -> %s", "RWTO_L1_PERIOD", "RWTO_NPFG_PERIOD");
+			return true;
+		}
+
+		if (strcmp("FW_L1_R_SLEW_MAX", node->name) == 0) {
+			strcpy(node->name, "FW_PN_R_SLEW_MAX");
+			PX4_INFO("copying %s -> %s", "FW_L1_R_SLEW_MAX", "FW_PN_R_SLEW_MAX");
+			return true;
+		}
+
+		if (strcmp("FW_L1_PERIOD", node->name) == 0) {
+			strcpy(node->name, "NPFG_PERIOD");
+			PX4_INFO("copying %s -> %s", "FW_L1_PERIOD", "NPFG_PERIOD");
+			return true;
+		}
+	}
 }
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	9766fcfb6f	[WIP] logger priority boost command line	2023-02-10 19:26:14 -05:00
Hamish Willee	edb6c635d5	EKF2_MAG_TYPE - fix typos (#20808 )	2023-02-10 14:39:57 +01:00
Konrad	b9f9f25b48	parameter_translation: Add parameter translation for renamed L1 parameters	2023-02-09 17:51:55 +01:00
Konrad	92277ebb96	FixedwingPositionControl: Explicitly set wind to zero when it is not valid.	2023-02-09 17:51:55 +01:00
Konrad	de4b139540	FwPosControl: Update behavior of navigating to a waypoint when the previous waypoint is not valid. Go along the line of the current aircraft position to the desired waypoint.	2023-02-09 17:51:55 +01:00
Konrad	d5025810b4	FixedWingPositionControl: remove get_nav_speed_2d function as npfg can handle this internally.	2023-02-09 17:51:55 +01:00
Konrad	6bdeb43e0d	fw_path_navigation: Remove explicit L1 mentioning.	2023-02-09 17:51:55 +01:00
Konrad	3e200bca0d	fw_pos_control_l1: renaming to fw_path_navigation, l1 control is not used anymore, use a more generic naming.	2023-02-09 17:51:55 +01:00
Konrad	aa3af7f707	fw_pos_control: purge L1 controller	2023-02-09 17:51:55 +01:00
Knut Hjorth	fbc80c9bf5	RTL fixes and improvements for VTOL vehicles (#21011 ) * 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.	2023-02-08 11:07:39 +01:00
Knut Hjorth	99cf1cfdfe	mavlink: use /dev/null as default stdin, stdout and stderr If 0, 1 and/or 2 file descriptors are not open when mavlink module starts (as might be the case for USB auto-start), use default /dev/null so that these numbers are not used by other other files.	2023-02-08 10:38:20 +01:00
Knut Hjorth	6c7ae3d845	mavlink: generate new log list for request start index 0 Instead of interpret a request for "more logs than currently exists" as a new request, use a request for index 0, which is more likely to be the first request.	2023-02-08 10:38:20 +01:00
KonradRudin	c5d041a2f7	Rearrange npfg use path input (#21071 ) * [npfg]: Remove the guideToPoint function and replace with guideToPath * [npfg]: remove unused navigateXXX functions * [npfg]: Move navigateXXX Function into FWPoscontrol * [FixedwingPositionControl]: Set default flaps and spoilers in attitude setpoint topic, and only change if necessary.	2023-02-08 08:54:00 +01:00
jonasbouchraiet	9ac27c9413	Update rtl_params.c	2023-02-07 22:37:19 -05:00
Beat Küng	83c8c79af5	commander failsafe: add API to defer failsafes	2023-02-07 19:27:51 -05:00
Beat Küng	a727bddc19	microdds_client: set queue depth for incoming topics according to msg definition Otherwise the FMU might miss publications from 2 different publishers at the same time.	2023-02-07 19:12:10 -05:00
Beat Küng	3f2336af32	navigator: add ModeCompleted signalling topic	2023-02-07 19:11:52 -05:00
Beat Küng	f05e8a699e	ROMFS: enable COM_LOW_BAT_ACT by default for SITL	2023-02-07 19:11:29 -05:00
Beat Küng	ebc1d7544e	battery_simulator: add support for failure injection For failsafe triggering in automated tests	2023-02-07 19:11:08 -05:00
Eric Katzfey	ddd1527305	Qurt PX4_INFO_RAW send to apps for display (#21080 )	2023-02-07 17:22:09 -05:00
Eric Katzfey	db24c2b233	Qshell static subscription (#21081 ) * Changed QShell uorb subscription to be static to avoid the duplicate sequence number error	2023-02-07 17:18:01 -05:00
Daniel Agar	7b3befded5	ekf2: disable new gravity fusion by default	2023-02-07 13:57:10 -05:00
Daniel Sahu	fa6fda6cce	ekf2: new gravity observation (#21038 ) Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>	2023-02-07 13:28:58 -05:00
alessandro	3e149ee6c5	FlightTaskAuto: landing position updates for precision landing (#20951 ) - precision landing works incorrectly, target position is not updated during the descent above target - _prepareLandSetpoints needs to update _land_position continuously Co-authored-by: kapacheuski <kapacheuski@gmail.com>	2023-02-07 12:07:26 -05:00
Jukka Laitinen	deb6053d56	Update status leds every time when prearm check status changes Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-02-07 08:02:32 +01:00
Christian Rauch	ef5761c223	add SPI to stm32f1	2023-02-07 07:54:38 +01:00
Christian Rauch	2f21c590b0	remove some vscode settings	2023-02-06 17:25:36 -05:00
JaeyoungLim	01c5b3934e	Tune GZ plane (rc_cessna) to fly nicely (#21077 ) * Increase control surface joint controller gains * Enable prearm mode and disable airpspeed checks for gz plane	2023-02-06 16:24:01 -05:00
Daniel Agar	661eb2adb4	lib/sensor_calibration: BiasCorrectedSensorOffset() don't incorporate thermal offsets - the thermal offsets are an optional correction applied to the raw data, so when updating an existing calibration offset with new learned bias we don't want this incorporated	2023-02-06 15:09:07 -05:00
Silvan Fuhrer	e153d1defc	ROMFS: fix PY asymmetry (motor 1 was wrongly placed twice as far from the CG as 0) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-04 16:08:40 +01:00
Daniel Agar	04d3e549f5	ekf2: resetQuatStateYaw() set _time_last_heading_fuse	2023-02-03 10:03:09 -05:00
Daniel Agar	66ad7fd06c	ekf2: include 0 timestamp checks in helpers (isTimedOut(), isRecent(), etc) - EKF isTimedOut(), isRecent(), and isNewestSampleRecent() need to handle the case where the timestamp has never been set - reset() more thoroughly reset fields (mainly impacts unit tests)	2023-02-03 10:00:51 -05:00
Daniel Agar	264a99fb77	ekf2: new EKF2_IMU_CTRL parameter and gyro bias inhibit mechanism - EKF2_AID_MASK accel bias inhibit moves to EKF2_IMU_CTRL	2023-02-03 09:52:24 -05:00
Daniel Agar	f668ea5aa6	ekf2: RingBuffer add reset method	2023-02-03 08:49:44 -05:00
Daniel Agar	e3d73cd837	ekf2: mag control reset mag_lpf on first sample	2023-02-02 16:12:09 -05:00
Michał Barciś	c2f13dbccf	setup/ubuntu.sh modified to correctly install all required dependencies for gazebo Signed-off-by: Michał Barciś <michal.barcis@tii.ae>	2023-02-02 10:02:47 -05:00
ShiauweiZhao	35080504f7	drivers/drv_sensor.h fix device type duplicate definition	2023-02-02 09:54:47 -05:00
Vincent Poon	a90bae9e50	ci: build and deploy kakuteh7v2 & Kakuteh7mini Adds Holybro kakuteh7v2 & Kakuteh7mini to Travis CI for building and deployment to S3 for QGC	2023-02-02 09:51:53 -05:00
Hamish Willee	2938db1c60	Apply suggestions from code review	2023-02-02 15:20:34 +01:00
Hamish Willee	7cea384404	Update src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c	2023-02-02 15:20:34 +01:00
Hamish Willee	d65e5969e1	FW_AT_MAN_AUX - define what an Aux input is	2023-02-02 15:20:34 +01:00
Silvan Fuhrer	ba1d02ee75	MPC: improve description of MPC_LAND_RC_HELP Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-02 14:31:22 +01:00
小光	849fbabc47	px4_mtd: the address of 'instances' will never be NULL (#21039 ) Signed-off-by: AuroraRAS <chplee@gmail.com>	2023-02-02 08:06:00 +01:00
Silvan Fuhrer	d9a4d1d5c4	Commander: use FW_AIRSPD_MAX as threshold for airspeed preflight checks Check fails if airspeed reading is above FW_AIRSPD_MAX. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 20:43:38 -05:00
Robbie Drage	cb4235887f	vscode: change cubeorange build target - Fixes typo in Cube Orange build target config Fixes issue #17745	2023-02-01 18:17:45 -05:00
Silvan Fuhrer	32cab66c44	Commander: hide hint to param in low position accuracy event for end users Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	9b3a28dff5	Commander: add local_position_accuracy_low flag, incl. warning and RTL Set this flag to true if local position is valid but accuracy low, such that the operator can be warned before system switches to position-failure failsafe. Additionally, switch to RTL if currently in Mission or Loiter to try to reach home or fly out of GNSS-denied area. Set low accuracy threshold to 50m by default for FW and VTOL. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	3ca126cc46	Commander: improve description of COM_FS EPH, EPV, VEL_EVH params and increase FW and VTOL defaults Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	cbc4c35bcf	Commander params: improve meta data for max flight time and wind Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Silvan Fuhrer	6d84da5cf1	Commander: add max flight time warning (starting at 90%) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Beat Küng	b1709743f7	commander: add wind or flight time limit exceeded mode requirement This prevents switching into any of these modes once the condition is set.	2023-02-01 08:48:09 +01:00
Beat Küng	a8628c9d9c	fix commander: clear takeoff_time only on disarm Otherwise the flight time restriction flag gets cleared too early, before disarming (which puts the vehicle into the previous mode and it might take off again).	2023-02-01 08:48:09 +01:00
Beat Küng	8e4c5884ec	fix commander: need to check for valid mode change even if already the same Fixes the following case: - user intention set to X - failsafe triggers, mode = Y - can_run for X becomes false - user tries to switch to X -> need to re-evaluate can_run	2023-02-01 08:48:09 +01:00
Beat Küng	7988491e37	failsafe simulator: improve spacing for multi-line checkboxes Reduces line-height to 100% in that case.	2023-02-01 08:48:09 +01:00