[WIP] logger priority boost command line

EKF2_MAG_TYPE - fix typos (#20808 )
parameter_translation: Add parameter translation for renamed L1 parameters
2026-05-29 05:10:04 +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
126 changed files with 929 additions and 966 deletions
@@ -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
@@ -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
@@ -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

 #
@@ -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
@@ -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
@@ -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[]);
@@ -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

@@ -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;
 };
@@ -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;
+		}
+	}
 }
@@ -167,7 +167,7 @@ void set_param_value_float(const std::string &name, float value)

 int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finished,
 		    bool user_override, uint8_t user_intended_mode, uint8_t vehicle_type,
-		    failsafe_flags_s status_flags)
+		    failsafe_flags_s status_flags, bool defer_failsafes)
 {
 	uint64_t time_ms = emscripten_date_now();
 	FailsafeBase::State state{};
@@ -177,6 +177,7 @@ int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finis
 	state.user_intended_mode = user_intended_mode;
 	state.vehicle_type = vehicle_type;
 	mode_util::getModeRequirements(vehicle_type, status_flags);
+	failsafe_instance.current().deferFailsafes(defer_failsafes, 0);
 	return failsafe_instance.current().update(time_ms * 1000, state, false, user_override, status_flags);
 }

@@ -206,6 +206,11 @@
                                <button onclick="moveRCSticks()">Move RC Sticks (user takeover request)</button>
                            </td>
                        </tr>
+                        <tr style="display:none;"> <!-- hide this by default -->
+                            <td><label for="defer_failsafes">Defer Failsafes:</label></td>
+                            <td><input type="checkbox" id="defer_failsafes" name="defer_failsafes"></td>
+                            <td></td>
+                        </tr>
                    </table>
                </div>
                <div class="box"><h3>Parameters</h3>
@@ -322,8 +327,9 @@
          var mission_finished = document.querySelector('input[id="mission_finished"]').checked;
          var intended_nav_state = document.querySelector('select[id="intended_nav_state"]').value;
          var vehicle_type = document.querySelector('select[id="vehicle_type"]').value;
+          var defer_failsafes = document.querySelector('input[id="defer_failsafes"]').checked;
          var updated_user_intended_mode = Module.failsafe_update(armed, vtol_in_transition_mode, mission_finished, user_override_requested, parseInt(intended_nav_state),
-              parseInt(vehicle_type), state);
+              parseInt(vehicle_type), state, defer_failsafes);
          user_override_requested = false;
          var action = Module.selected_action();
          action_str = Module.action_str(action);
@@ -392,8 +392,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,


 	CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
-		       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
-	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL));
+		       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm).cannotBeDeferred());
+	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());

 	// trigger RTL if low position accurancy is detected
 	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
@@ -401,7 +401,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
 	}

-	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
+	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());

 	// Battery
 	CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
@@ -434,7 +434,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	if ((_armed_time != 0)
 	    && (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
 	   ) {
-		CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm);
+		CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, ActionOptions(Action::Disarm).cannotBeDeferred());
 	}

 	if ((_armed_time != 0)
@@ -442,10 +442,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		+ static_cast<hrt_abstime>((_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s))
 	   ) {
 		// This handles the case where something fails during the early takeoff phase
-		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Disarm);
+		CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Disarm).cannotBeDeferred());

 	} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
-		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate);
+		CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());

 	} else {
 		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
@@ -460,7 +460,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode);
 	_last_state_mode_fallback = checkFailsafe(_caller_id_mode_fallback, _last_state_mode_fallback,
 				    mode_fallback_action != Action::None,
-				    ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
+				    ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always).cannotBeDeferred());
 }

 void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags)
@@ -62,7 +62,7 @@ protected:
 			       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));

 		_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
-						 && status_flags.fd_critical_failure, Action::Terminate);
+						 && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
 	}

 	Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override
@@ -258,3 +258,114 @@ TEST_F(FailsafeTest, takeover_denied)
 	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
 }

+TEST_F(FailsafeTest, defer)
+{
+	FailsafeTester failsafe(nullptr);
+
+	failsafe_flags_s failsafe_flags{};
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 3847124342;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+
+	const int defer_timeout_s = 10;
+	failsafe.deferFailsafes(true, defer_timeout_s);
+	ASSERT_TRUE(failsafe.getDeferFailsafes());
+	ASSERT_FALSE(failsafe.failsafeDeferred());
+	// GCS connection lost -> deferred
+	time += 10_ms;
+	failsafe_flags.gcs_connection_lost = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_TRUE(failsafe.getDeferFailsafes());
+	ASSERT_TRUE(failsafe.failsafeDeferred());
+
+	// Wait a bit, still deferred
+	time += 5_s;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_TRUE(failsafe.getDeferFailsafes());
+	ASSERT_TRUE(failsafe.failsafeDeferred());
+
+	// Wait a bit, don't defer anymore -> failsafe triggered (hold)
+	time += 1_s;
+	failsafe.deferFailsafes(false, 0);
+	ASSERT_FALSE(failsafe.getDeferFailsafes());
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+	ASSERT_FALSE(failsafe.getDeferFailsafes());
+	ASSERT_FALSE(failsafe.failsafeDeferred());
+
+	// Cannot enable while in failsafe
+	ASSERT_FALSE(failsafe.deferFailsafes(true, 0));
+
+	// Still in hold
+	time += 4_s;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+
+	// Now changed to descend
+	time += 4_s;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
+
+	// Clear failsafe
+	failsafe_flags.gcs_connection_lost = false;
+	time += 10_ms;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+
+	// Defer and trigger timeout
+	failsafe.deferFailsafes(true, defer_timeout_s);
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_TRUE(failsafe.failsafeDeferred());
+	time += 1_s * defer_timeout_s + 10_ms;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+	ASSERT_FALSE(failsafe.failsafeDeferred());
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = false;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+	// Defer and clear failsafe -> no action
+	failsafe.deferFailsafes(true, defer_timeout_s);
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_TRUE(failsafe.failsafeDeferred());
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = false;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_FALSE(failsafe.failsafeDeferred());
+	failsafe.deferFailsafes(false, defer_timeout_s);
+	time += 10_ms;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+	// Defer and trigger non-deferrable failsafe
+	failsafe.deferFailsafes(true, defer_timeout_s);
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_TRUE(failsafe.failsafeDeferred());
+	time += 10_ms;
+	failsafe_flags.fd_motor_failure = true;
+	failsafe_flags.fd_critical_failure = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
+	ASSERT_FALSE(failsafe.failsafeDeferred());
+}
@@ -70,7 +70,14 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
 		removeActions(ClearCondition::OnModeChangeOrDisarm);
 	}

-	updateDelay(time_us - _last_update);
+	if (_defer_failsafes && _failsafe_defer_started != 0 && _defer_timeout > 0
+	    && time_us > _failsafe_defer_started + _defer_timeout) {
+		_defer_failsafes = false;
+	}
+
+	if (_failsafe_defer_started == 0) {
+		updateDelay(time_us - _last_update);
+	}

 	checkStateAndMode(time_us, state, status_flags);
 	removeNonActivatedActions();
@@ -79,7 +86,8 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
 	SelectedActionState action_state{};
 	getSelectedAction(state, status_flags, user_intended_mode_updated, rc_sticks_takeover_request, action_state);

-	updateDelay(time_us - _last_update, action_state.delayed_action != Action::None);
+	updateStartDelay(time_us - _last_update, action_state.delayed_action != Action::None);
+	updateFailsafeDeferState(time_us, action_state.failsafe_deferred);

 	// Notify user if the action is worse than before, or a new action got added
 	if (action_state.action > _selected_action || (action_state.action != Action::None && _notification_required)) {
@@ -98,7 +106,19 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
 	return _last_user_intended_mode;
 }

-void FailsafeBase::updateDelay(const hrt_abstime &dt, bool delay_active)
+void FailsafeBase::updateFailsafeDeferState(const hrt_abstime &time_us, bool defer)
+{
+	if (defer) {
+		if (_failsafe_defer_started == 0) {
+			_failsafe_defer_started = time_us;
+		}
+
+	} else {
+		_failsafe_defer_started = 0;
+	}
+}
+
+void FailsafeBase::updateStartDelay(const hrt_abstime &dt, bool delay_active)
 {
 	// Ensure that even with a toggling state the delayed action is executed at some point.
 	// This is done by increasing the delay slower than reducing it.
@@ -411,6 +431,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 	returned_state.action = Action::None;
 	Action &selected_action = returned_state.action;
 	UserTakeoverAllowed allow_user_takeover = UserTakeoverAllowed::Always;
+	bool allow_failsafe_to_be_deferred{true};

 	// Select the worst action based on the current active actions
 	for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
@@ -426,9 +447,19 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 				selected_action = cur_action.action;
 				returned_state.cause = cur_action.cause;
 			}
+
+			if (!cur_action.can_be_deferred) {
+				allow_failsafe_to_be_deferred = false;
+			}
 		}
 	}

+	if (_defer_failsafes && allow_failsafe_to_be_deferred && selected_action != Action::None) {
+		returned_state.failsafe_deferred = selected_action > Action::Warn;
+		returned_state.action = Action::None;
+		return;
+	}
+
 	// Check if we should enter delayed Hold
 	if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly
 	    && selected_action != Action::Disarm && selected_action != Action::Terminate) {
@@ -627,3 +658,23 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
 		(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
 		((status_flags.mode_req_other & mode_mask) == 0);
 }
+
+bool FailsafeBase::deferFailsafes(bool enabled, int timeout_s)
+{
+	if (enabled && _selected_action > Action::Warn) {
+		return false;
+	}
+
+	if (timeout_s == 0) {
+		_defer_timeout = DEFAULT_DEFER_TIMEOUT;
+
+	} else if (timeout_s < 0) {
+		_defer_timeout = 0;
+
+	} else {
+		_defer_timeout = timeout_s * 1_s;
+	}
+
+	_defer_failsafes = enabled;
+	return true;
+}
@@ -39,12 +39,16 @@

 #include <cstddef>

+using namespace time_literals;
+
 #define CHECK_FAILSAFE(status_flags, flag_name, options) \
 	checkFailsafe((int)offsetof(failsafe_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options)

 class FailsafeBase: public ModuleParams
 {
 public:
+	static constexpr hrt_abstime DEFAULT_DEFER_TIMEOUT = 30_s;
+
 	enum class Action : uint8_t {
 		// Actions further down take precedence
 		None,
@@ -149,6 +153,16 @@ public:

 	bool userTakeoverActive() const { return _user_takeover_active; }

+	/**
+	 * Defer all failsafes that can be deferred. Can be used to avoid triggering failsafes in critical situations.
+	 * @param enabled
+	 * @param timeout_s timeout in seconds, if set to 0, DEFAULT_DEFER_TIMEOUT is used, -1=no timeout
+	 * @return true on success, false if failsafe already active
+	 */
+	bool deferFailsafes(bool enabled, int timeout_s);
+	bool getDeferFailsafes() const { return _defer_failsafes; }
+	bool failsafeDeferred() const { return _failsafe_defer_started != 0; }
+
 protected:
 	enum class UserTakeoverAllowed {
 		Always, ///< allow takeover (immediately)
@@ -162,6 +176,7 @@ protected:
 		ActionOptions &allowUserTakeover(UserTakeoverAllowed allow = UserTakeoverAllowed::Auto) { allow_user_takeover = allow; return *this; }
 		ActionOptions &clearOn(ClearCondition clear_condition_) { clear_condition = clear_condition_; return *this; }
 		ActionOptions &causedBy(Cause cause_) { cause = cause_; return *this; }
+		ActionOptions &cannotBeDeferred() { can_be_deferred = false; return *this; }

 		bool valid() const { return id != -1; }
 		void setInvalid() { id = -1; }
@@ -170,6 +185,7 @@ protected:
 		ClearCondition clear_condition{ClearCondition::WhenConditionClears};
 		Cause cause{Cause::Generic};
 		UserTakeoverAllowed allow_user_takeover{UserTakeoverAllowed::Auto};
+		bool can_be_deferred{true};

 		bool state_failure{false}; ///< used when the clear_condition isn't set to clear immediately
 		bool activated{false}; ///< true if checkFailsafe was called during current update
@@ -182,6 +198,7 @@ protected:
 		Cause cause{Cause::Generic};
 		uint8_t updated_user_intended_mode{};
 		bool user_takeover{false};
+		bool failsafe_deferred{false};
 	};

 	virtual void checkStateAndMode(const hrt_abstime &time_us, const State &state,
@@ -232,7 +249,9 @@ private:

 	bool actionAllowsUserTakeover(Action action) const;

-	void updateDelay(const hrt_abstime &dt, bool delay_active);
+	void updateStartDelay(const hrt_abstime &dt, bool delay_active);
+
+	void updateFailsafeDeferState(const hrt_abstime &time_us, bool defer);

 	static constexpr int max_num_actions{8};
 	ActionOptions _actions[max_num_actions]; ///< currently active actions
@@ -245,6 +264,10 @@ private:
 	bool _user_takeover_active{false};
 	bool _notification_required{false};

+	bool _defer_failsafes{false};
+	hrt_abstime _defer_timeout{0};
+	hrt_abstime _failsafe_defer_started{0};
+
 	hrt_abstime _current_start_delay{0}; ///< _current_delay is set to this value when starting the delay
 	hrt_abstime _current_delay{0}; ///< If > 0, stay in Hold, and take action once delay reaches 0

@@ -489,9 +489,10 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
 /**
 * Type of magnetometer fusion
 *
- * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. The fuson of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
+ * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector.
+ * The fusion of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
 * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable.
- * If set to 'Magnetic heading' magnetic heading fusion is used at all times
+ * If set to 'Magnetic heading' magnetic heading fusion is used at all times.
 * If set to '3-axis' 3-axis field fusion is used at all times.
 * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
 * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
@@ -35,15 +35,14 @@ add_subdirectory(launchdetection)
 add_subdirectory(runway_takeoff)

 px4_add_module(
-	MODULE modules__fw_pos_control_l1
-	MAIN fw_pos_control_l1
+	MODULE modules__fw_path_navigation
+	MAIN fw_path_navigation
 	SRCS
 		FixedwingPositionControl.cpp
 		FixedwingPositionControl.hpp
 	DEPENDS
-		l1
 		launchdetection
-    npfg
+    		npfg
 		runway_takeoff
 		SlewRate
 		tecs
@@ -100,12 +100,6 @@ FixedwingPositionControl::parameters_update()
 		param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
 	}

-	// L1 control parameters
-	_l1_control.set_l1_damping(_param_fw_l1_damping.get());
-	_l1_control.set_l1_period(_param_fw_l1_period.get());
-	_l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get()));
-	_l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get()));
-
 	// NPFG parameters
 	_npfg.setPeriod(_param_npfg_period.get());
 	_npfg.setDamping(_param_npfg_damping.get());
@@ -119,7 +113,7 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
 	_npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
 	_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
-	_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
+	_npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max.get()));
 	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());

 	// TECS parameters
@@ -309,6 +303,11 @@ FixedwingPositionControl::wind_poll()
 		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
 		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
 	}
+
+	if (!_wind_valid) {
+		_wind_vel(0) = 0.f;
+		_wind_vel(1) = 0.f;
+	}
 }

 void
@@ -399,7 +398,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 	}

 	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
-	if (!_l1_control.circle_mode() && !using_npfg_with_wind_estimate()) {
+	if (!_wind_valid) {
 		/*
 		 * This error value ensures that a plane (as long as its throttle capability is
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
@@ -516,50 +515,33 @@ FixedwingPositionControl::status_publish()
 	pos_ctrl_status.nav_roll = _att_sp.roll_body;
 	pos_ctrl_status.nav_pitch = _att_sp.pitch_body;

-	if (_l1_control.has_guidance_updated()) {
-		pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
+	npfg_status_s npfg_status = {};

-		pos_ctrl_status.target_bearing = _l1_control.target_bearing();
-		pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
+	npfg_status.wind_est_valid = _wind_valid;

-	} else {
-		pos_ctrl_status.nav_bearing = NAN;
-		pos_ctrl_status.target_bearing = NAN;
-		pos_ctrl_status.xtrack_error = NAN;
-	}
+	const float bearing = _npfg.getBearing(); // dont repeat atan2 calc

-	if (_param_fw_use_npfg.get()) {
-		npfg_status_s npfg_status = {};
+	pos_ctrl_status.nav_bearing = bearing;
+	pos_ctrl_status.target_bearing = _target_bearing;
+	pos_ctrl_status.xtrack_error = _npfg.getTrackError();
+	pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);

-		npfg_status.wind_est_valid = _wind_valid;
+	npfg_status.lat_accel = _npfg.getLateralAccel();
+	npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
+	npfg_status.heading_ref = _npfg.getHeadingRef();
+	npfg_status.bearing = bearing;
+	npfg_status.bearing_feas = _npfg.getBearingFeas();
+	npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
+	npfg_status.signed_track_error = _npfg.getTrackError();
+	npfg_status.track_error_bound = _npfg.getTrackErrorBound();
+	npfg_status.airspeed_ref = _npfg.getAirspeedRef();
+	npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
+	npfg_status.adapted_period = _npfg.getAdaptedPeriod();
+	npfg_status.p_gain = _npfg.getPGain();
+	npfg_status.time_const = _npfg.getTimeConst();
+	npfg_status.timestamp = hrt_absolute_time();

-		const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
-
-		pos_ctrl_status.nav_bearing = bearing;
-		pos_ctrl_status.target_bearing = _npfg.targetBearing();
-		pos_ctrl_status.xtrack_error = _npfg.getTrackError();
-		pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
-
-		npfg_status.lat_accel = _npfg.getLateralAccel();
-		npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
-		npfg_status.heading_ref = _npfg.getHeadingRef();
-		npfg_status.bearing = bearing;
-		npfg_status.bearing_feas = _npfg.getBearingFeas();
-		npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
-		npfg_status.signed_track_error = _npfg.getTrackError();
-		npfg_status.track_error_bound = _npfg.getTrackErrorBound();
-		npfg_status.airspeed_ref = _npfg.getAirspeedRef();
-		npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
-		npfg_status.adapted_period = _npfg.getAdaptedPeriod();
-		npfg_status.p_gain = _npfg.getPGain();
-		npfg_status.time_const = _npfg.getTimeConst();
-		npfg_status.timestamp = hrt_absolute_time();
-
-		_npfg_status_pub.publish(npfg_status);
-
-	} else {
-		pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
-	}
+	_npfg_status_pub.publish(npfg_status);

 	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
 				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
@@ -850,7 +832,7 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 		if (!PX4_ISFINITE(_transition_waypoint(0))) {
 			double lat_transition, lon_transition;

-			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
+			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
 			// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
 			// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
 			const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
@@ -895,9 +877,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		_att_sp.thrust_body[0] = 0.0f;
 		_att_sp.roll_body = 0.0f;
 		_att_sp.pitch_body = radians(_param_fw_psp_off.get());
-		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
-
 		break;

 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
@@ -957,9 +936,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 	}

 	_att_sp.pitch_body = get_tecs_pitch();
-
-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 }

 void
@@ -986,9 +962,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)

 	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
 	_att_sp.pitch_body = get_tecs_pitch();
-
-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 }

 uint8_t
@@ -1005,7 +978,7 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 	/* current waypoint (the one currently heading for) */
 	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);

-	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
+	const float acc_rad = _npfg.switchDistance(500.0f);

 	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
 	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@@ -1036,15 +1009,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 				// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
 				position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 			}
-
-		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-			// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER (NPFG handles this internally)
-			if ((dist >= 0.f)
-			    && (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))
-			    && !_param_fw_use_npfg.get()) {
-				// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
-				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-			}
 		}
 	}

@@ -1055,7 +1019,7 @@ void
 FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
-	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
+	const float acc_rad = _npfg.switchDistance(500.0f);
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};

@@ -1067,10 +1031,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		prev_wp(1) = pos_sp_prev.lon;

 	} else {
-		// No valid previous waypoint, go for the current wp.
-		// This is automatically handled by the L1/NPFG libraries.
-		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = pos_sp_curr.lon;
+		// No valid previous waypoint, go along the line between aircraft and current waypoint
+		prev_wp = curr_pos;
 	}

 	float tecs_fw_thr_min;
@@ -1129,34 +1091,25 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
 	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));

-	if (_param_fw_use_npfg.get()) {
-		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);

-		if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
-			// Navigate directly on position setpoint and path tangent
-			matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-			float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
-					  0.0f;
-			_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
-						  _wind_vel, curvature);
-
-		} else {
-			_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
-		}
-
-		_att_sp.roll_body = _npfg.getRollSetpoint();
-		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+	if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
+		// Navigate directly on position setpoint and path tangent
+		matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
+		float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
+				  0.0f;
+		navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
+				    _wind_vel, curvature);

 	} else {
-		_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
-		_att_sp.roll_body = _l1_control.get_roll_setpoint();
+		navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 	}

-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
@@ -1198,23 +1151,15 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
 				_param_fw_airspd_min.get(), ground_speed);

-	if (_param_fw_use_npfg.get()) {
-		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateBearing(_target_bearing, ground_speed, _wind_vel);
-		_att_sp.roll_body = _npfg.getRollSetpoint();
-		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-
-	} else {
-		_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
-		_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	}
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

 	_att_sp.yaw_body = _yaw;

-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
-
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
 				   target_airspeed,
@@ -1244,10 +1189,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		prev_wp(1) = pos_sp_prev.lon;

 	} else {
-		// No valid previous waypoint, go for the current wp.
-		// This is automatically handled by the L1/NPFG libraries.
-		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = pos_sp_curr.lon;
+		// No valid previous waypoint, go along the line between aircraft and current waypoint
+		prev_wp = curr_pos;
 	}

 	float airspeed_sp = -1.f;
@@ -1279,10 +1222,14 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		loiter_radius = _param_nav_loiter_rad.get();
 	}

-	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))};
+	Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local};
+
+	const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500);

 	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
-	    && in_circle_mode && _param_fw_lnd_earlycfg.get()) {
+	    && close_to_circle && _param_fw_lnd_earlycfg.get()) {
 		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 		// landing airspeed and potentially tighter altitude control) already such that we don't
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
@@ -1291,32 +1238,18 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;

-	} else {
-		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}

 	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
 				ground_speed);

-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
-				     get_nav_speed_2d(ground_speed),
-				     _wind_vel);
-		_att_sp.roll_body = _npfg.getRollSetpoint();
-		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-
-	} else {
-		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius,
-					    pos_sp_curr.loiter_direction_counter_clockwise,
-					    get_nav_speed_2d(ground_speed));
-		_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	}
+	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
+		       ground_speed,
+		       _wind_vel);
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

@@ -1406,9 +1339,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 		// tune up the lateral position control guidance when on the ground
 		if (_att_sp.fw_control_yaw_wheel) {
-			_npfg.setPeriod(_param_rwto_l1_period.get());
-			_l1_control.set_l1_period(_param_rwto_l1_period.get());
-
+			_npfg.setPeriod(_param_rwto_npfg_period.get());
 		}

 		const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
@@ -1423,37 +1354,20 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
 		}

-		if (_param_fw_use_npfg.get()) {
-			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
-						  _wind_vel, 0.0f);
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
+				    _wind_vel, 0.0f);

-			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
+		_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());

-			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

-			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
-			const float bearing = _npfg.getBearing();
+		// use npfg's bearing to commanded course, controlled via yaw angle while on runway
+		const float bearing = _npfg.getBearing();

-			// heading hold mode will override this bearing setpoint
-			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
-
-		} else {
-			// make a fake waypoint in the direction of the takeoff bearing
-			const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
-
-			_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
-
-			const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
-			_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);
-
-			// use l1's nav bearing to command course, controlled via yaw angle while on runway
-			const float bearing = _l1_control.nav_bearing();
-
-			// heading hold mode will override this bearing setpoint
-			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
-		}
+		// heading hold mode will override this bearing setpoint
+		_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);

 		// update tecs
 		const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
@@ -1485,7 +1399,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 		// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
-		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

 	} else {
 		/* Perform launch detection */
@@ -1529,20 +1442,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		    && _param_fw_laun_detcn_on.get()) {
 			/* Launch has been detected, hence we have to control the plane. */

-			if (_param_fw_use_npfg.get()) {
-				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
-							  0.0f);
-				_att_sp.roll_body = _npfg.getRollSetpoint();
-				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
+					    0.0f);
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

-			} else {
-				// make a fake waypoint in the direction of the takeoff bearing
-				const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
-				_l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed);
-				_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			}

 			const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ?
 							   _param_fw_thr_idle.get() : _param_fw_thr_max.get();
@@ -1578,9 +1484,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
 		}

-		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
-
 		launch_detection_status_s launch_detection_status;
 		launch_detection_status.timestamp = now;
 		launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1676,41 +1579,20 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */

 		// tune up the lateral position control guidance when on the ground
-		const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
+		const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() +
 						      (1.0f - flare_ramp_interpolator) * _param_npfg_period.get();
-		const float ground_roll_l1_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
-						    (1.0f - flare_ramp_interpolator) * _param_fw_l1_period.get();
 		_npfg.setPeriod(ground_roll_npfg_period);
-		_l1_control.set_l1_period(ground_roll_l1_period);

 		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;

-		if (_param_fw_use_npfg.get()) {
-			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
-			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-			_att_sp.roll_body = _npfg.getRollSetpoint();
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+		_att_sp.roll_body = _npfg.getRollSetpoint();

-			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
-			_att_sp.yaw_body = _npfg.getBearing();
-
-		} else {
-			// make a fake waypoint beyond the land point in the direction of the landing approach bearing
-			// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
-
-			const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
-						local_position - local_approach_entrance);
-
-			const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
-							  landing_approach_vector.unit_or_zero();
-
-			_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-
-			// use l1's nav bearing to command course, controlled via yaw angle while on runway
-			_att_sp.yaw_body = _l1_control.nav_bearing();
-		}
+		// use npfg's bearing to commanded course, controlled via yaw angle while on runway
+		_att_sp.yaw_body = _npfg.getBearing();

 		/* longitudinal guidance */

@@ -1782,26 +1664,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo

 		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;

-		if (_param_fw_use_npfg.get()) {
-			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
-			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-			_att_sp.roll_body = _npfg.getRollSetpoint();
-
-		} else {
-			// make a fake waypoint beyond the land point in the direction of the landing approach bearing
-			// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
-
-			const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
-						local_position - local_approach_entrance);
-
-			const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
-							  landing_approach_vector.unit_or_zero();
-
-			_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-		}
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+		_att_sp.roll_body = _npfg.getRollSetpoint();

 		/* longitudinal guidance */

@@ -1889,11 +1756,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,

 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
 	_att_sp.pitch_body = get_tecs_pitch();
-
-	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
-	// through attitdue setpoints
-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 }

 void
@@ -1962,18 +1824,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
 						 prev_wp(1));

-			if (_param_fw_use_npfg.get()) {
-				_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
-				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-				_att_sp.roll_body = _npfg.getRollSetpoint();
-				calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
-
-			} else {
-				/* populate l1 control setpoint */
-				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
-				_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			}
+			_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+			calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;

 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}
@@ -1999,7 +1854,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,

 		// do slew rate limiting on roll if enabled
 		float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
-		const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
+		const float roll_rate_slew_rad = radians(_param_fw_pn_r_slew_max.get());

 		if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
 			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
@@ -2012,11 +1867,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,

 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
 	_att_sp.pitch_body = get_tecs_pitch();
-
-	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
-	// through attitdue setpoints
-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 }

 float
@@ -2216,12 +2066,7 @@ FixedwingPositionControl::Run()
 		update_in_air_states(_local_pos.timestamp);

 		// update lateral guidance timesteps for slewrates
-		if (_param_fw_use_npfg.get()) {
-			_npfg.setDt(control_interval);
-
-		} else {
-			_l1_control.set_dt(control_interval);
-		}
+		_npfg.setDt(control_interval);

 		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
@@ -2229,9 +2074,10 @@ FixedwingPositionControl::Run()

 		// restore lateral-directional guidance parameters (changed in takeoff mode)
 		_npfg.setPeriod(_param_npfg_period.get());
-		_l1_control.set_l1_period(_param_fw_l1_period.get());

 		_att_sp.reset_integral = false;
+		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
+		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw_wheel = false;
@@ -2288,9 +2134,6 @@ FixedwingPositionControl::Run()
 		case FW_POSCTRL_MODE_OTHER: {
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());

-				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
-
 				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);

 				break;
@@ -2326,8 +2169,6 @@ FixedwingPositionControl::Run()

 				}
 			}
-
-			_l1_control.reset_has_guidance_updated();
 		}

 		perf_end(_loop_perf);
@@ -2364,35 +2205,6 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }

-bool FixedwingPositionControl::using_npfg_with_wind_estimate() const
-{
-	// high wind mitigation is handled by NPFG if the wind estimate is valid
-	return _param_fw_use_npfg.get() && _wind_valid;
-}
-
-Vector2f
-FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
-{
-
-	Vector2f nav_speed_2d{ground_speed};
-
-	if (_airspeed_valid && !using_npfg_with_wind_estimate()) {
-		// l1 navigation logic breaks down when wind speed exceeds max airspeed
-		// compute 2D groundspeed from airspeed-heading projection
-		const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
-
-		// angle between air_speed_2d and ground_speed
-		const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
-
-		// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
-		if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
-			nav_speed_2d = air_speed_2d;
-		}
-	}
-
-	return nav_speed_2d;
-}
-
 float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
 		float throttle_max)
 {
@@ -2714,14 +2526,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo

 	Vector2f current_setpoint;

-	if (!_param_fw_use_npfg.get()) {
-		if (_global_local_proj_ref.isInitialized()) {
-			current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
-		}
-
-	} else {
-		current_setpoint = _npfg.getClosestPoint();
-	}
+	current_setpoint = _closest_point_on_path;

 	local_position_setpoint.x = current_setpoint(0);
 	local_position_setpoint.y = current_setpoint(1);
@@ -2759,6 +2564,104 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 	_orbit_status_pub.publish(orbit_status);
 }

+void FixedwingPositionControl::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
+
+	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
+	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
+
+	if (vector_A_to_B.norm() < FLT_EPSILON) {
+		// the waypoints are on top of each other and should be considered as a
+		// single waypoint, fly directly to it
+		if (vector_A_to_vehicle.norm() > FLT_EPSILON) {
+			vector_A_to_B = -vector_A_to_vehicle;
+
+		} else {
+			// Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output.
+			return;
+		}
+
+
+	} else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) {
+		// we are in front of waypoint A, fly directly to it until we are within switch distance.
+
+		if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) {
+			vector_A_to_B = -vector_A_to_vehicle;
+		}
+	}
+
+	// track the line segment
+	Vector2f unit_path_tangent{vector_A_to_B.normalized()};
+	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
+	_closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
+	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);
+} // navigateWaypoints
+
+void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
+		float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	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
+	const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
+
+	float path_curvature = loiter_direction_multiplier / radius;
+	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
+	_closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center;
+	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent,
+			  loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature);
+} // navigateLoiter
+
+
+void FixedwingPositionControl::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)
+{
+	const Vector2f unit_path_tangent{tangent_setpoint.normalized()};
+	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
+	_closest_point_on_path = position_setpoint;
+	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature);
+} // navigatePathTangent
+
+void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
+		const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+
+	const Vector2f unit_path_tangent = Vector2f{cosf(bearing), sinf(bearing)};
+	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
+	_closest_point_on_path = vehicle_pos;
+	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f);
+} // navigateBearing
+
 int FixedwingPositionControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;
@@ -2804,11 +2707,11 @@ int FixedwingPositionControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-fw_pos_control_l1 is the fixed wing position controller.
+fw_path_navigation is the fixed wing path navigation.

 )DESCR_STR");

-	PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller");
+	PRINT_MODULE_USAGE_NAME("fw_path_navigation", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
@@ -2816,7 +2719,7 @@ fw_pos_control_l1 is the fixed wing position controller.
 	return 0;
 }

-extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[])
+extern "C" __EXPORT int fw_path_navigation_main(int argc, char *argv[])
 {
 	return FixedwingPositionControl::main(argc, argv);
 }
@@ -33,11 +33,11 @@


 /**
- * @file fw_pos_control_l1_main.hpp
- * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
+ * @file fw_path_navigation_main.hpp
+ * Implementation of a generic path navigation. Outputs a bank / roll
 * angle, equivalent to a lateral motion (for copters and rovers).
 *
- * The implementation for the controllers is in the ECL library. This class only
+ * The implementation for the controllers is in a separate library. This class only
 * interfaces to the library.
 *
 * @author Lorenz Meier <lorenz@px4.io>
@@ -55,7 +55,6 @@

 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
-#include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
 #include <lib/mathlib/mathlib.h>
@@ -157,10 +156,6 @@ static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
 // altitude while waiting for navigator to flag it exceeded
 static constexpr float kClearanceAltitudeBuffer = 10.0f;

-// [m] a very large number to hopefully avoid the "fly back" case in L1 waypoint following logic once passed the second
-// waypoint in the segment. this is unecessary with NPFG.
-static constexpr float L1_VIRTUAL_TAKEOFF_WP_DIST = 1.0e6f;
-
 // [m/s] maximum rate at which the touchdown position can be nudged
 static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f;

@@ -412,8 +407,8 @@ private:

 	// LATERAL-DIRECTIONAL GUIDANCE

-	// L1 guidance - lateral-directional position control
-	ECL_L1_Pos_Controller _l1_control;
+	// CLosest point on path to track
+	matrix::Vector2f _closest_point_on_path;

 	// nonlinear path following guidance - lateral-directional position control
 	NPFG _npfg;
@@ -655,19 +650,6 @@ private:
 	void reset_takeoff_state();
 	void reset_landing_state();

-	bool using_npfg_with_wind_estimate() const;
-
-	/**
-	 * @brief Returns the velocity vector to be input in the lateral-directional guidance laws.
-	 *
-	 * Replaces the ground velocity vector with an air velocity vector depending on the wind condition and whether
-	 * NPFG or L1 are being used for horizontal position control.
-	 *
-	 * @param ground_speed Vehicle ground velocity vector [m/s]
-	 * @return Velocity vector to control with lateral-directional guidance [m/s]
-	 */
-	Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
-
 	/**
 	 * @brief Decides which control mode to execute.
 	 *
@@ -773,6 +755,68 @@ private:
 	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
 				   const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point);

+	/*
+	 * 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 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] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @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(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel,
+			     const matrix::Vector2f &wind_vel);
+
 	DEFINE_PARAMETERS(

 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@@ -782,12 +826,9 @@ private:

 		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,

-		(ParamFloat<px4::params::FW_L1_DAMPING>) _param_fw_l1_damping,
-		(ParamFloat<px4::params::FW_L1_PERIOD>) _param_fw_l1_period,
-		(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
+		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
 		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,

-		(ParamBool<px4::params::FW_USE_NPFG>) _param_fw_use_npfg,
 		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
 		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
 		(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
@@ -861,7 +902,7 @@ private:
 		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
 		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,

-		(ParamFloat<px4::params::RWTO_L1_PERIOD>) _param_rwto_l1_period,
+		(ParamFloat<px4::params::RWTO_NPFG_PERIOD>) _param_rwto_npfg_period,
 		(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,

 		(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_PATH_NAVIGATION
+	bool "fw_path_navigation"
+	default n
+	---help---
+		Enable support for fw_path_navigation
+
+menuconfig USER_FW_PATH_NAVIGATION
+	bool "fw_path_navigation running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_PATH_NAVIGATION
+	---help---
+		Put fw_path_navigation in userspace memory
@@ -32,49 +32,7 @@
 ****************************************************************************/

 /**
- * @file fw_pos_control_l1_params.c
- *
- * Parameters defined by the L1 position control task
- *
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-/*
- * Controller parameters, accessible via MAVLink
- */
-
-/**
- * L1 period
- *
- * Used to determine the L1 gain and controller time constant. This parameter is
- * proportional to the L1 distance (which points ahead of the aircraft on the path
- * it is following). A value of 18-25 seconds works for most aircraft. Shorten
- * slowly during tuning until response is sharp without oscillation.
- *
- * @unit s
- * @min 7.0
- * @max 50.0
- * @decimal 1
- * @increment 0.5
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f);
-
-/**
- * L1 damping
- *
- * Damping factor for L1 control.
- *
- * @min 0.6
- * @max 0.9
- * @decimal 2
- * @increment 0.05
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
-
-/**
- * L1 controller roll slew rate limit.
+ * Path navigation roll slew rate limit.
 *
 * The maximum change in roll angle setpoint per second.
 *
@@ -82,19 +40,9 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 * @min 0
 * @decimal 0
 * @increment 1
- * @group FW L1 Control
+ * @group FW Path Control
 */
-PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f);
-
-/**
- * Use NPFG as lateral-directional guidance law for fixed-wing vehicles
- *
- * Replaces L1.
- *
- * @boolean
- * @group FW NPFG Control
- */
-PARAM_DEFINE_INT32(FW_USE_NPFG, 1);
+PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f);

 /**
 * NPFG period
@@ -205,8 +153,7 @@ PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
 * NPFG switch distance multiplier
 *
 * Multiplied by the track error boundary to determine when the aircraft switches
- * to the next waypoint and/or path segment. Should be less than 1. 1/pi (0.32)
- * sets the switch distance equivalent to that of the L1 controller.
+ * to the next waypoint and/or path segment. Should be less than 1.
 *
 * @min 0.1
 * @max 1.0
@@ -295,7 +242,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f);
 * @max 65.0
 * @decimal 1
 * @increment 0.5
- * @group FW L1 Control
+ * @group FW Path Control
 */
 PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);

@@ -379,7 +326,7 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 * @max 30.0
 * @decimal 1
 * @increment 0.5
- * @group FW L1 Control
+ * @group FW Path Control
 */
 PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);

@@ -798,7 +745,7 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
 * @max 3
 * @bit 0 Alternative stick configuration (height rate on throttle stick, airspeed on pitch stick)
 * @bit 1 Enable airspeed setpoint via sticks in altitude and position flight mode
- * @group FW L1 Control
+ * @group FW Path Control
 */
 PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);

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