publish platform position on uOrb topic.

reused SensorGps.msg with new topic platform_gps_position. Currently only latitude_deg, longigude_deg and altitude_ellipsoid are set. check out with listener platform_gps_position.
Moving platform: random rocking motion
2026-05-24 06:47:36 +08:00 · 2025-03-05 08:57:37 +01:00 · 2025-03-03 14:52:33 +01:00 · 2025-03-03 10:37:33 +01:00 · 2025-02-28 16:56:54 +01:00 · 2025-02-28 10:21:01 +01:00
103 changed files with 2812 additions and 2121 deletions
@@ -75,7 +75,7 @@
 [submodule "Tools/simulation/gz"]
 	path = Tools/simulation/gz
 	url = https://github.com/PX4/PX4-gazebo-models.git
-	branch = main
+	branch = moving_platform_world
 [submodule "boards/modalai/voxl2/libfc-sensor-api"]
 	path = boards/modalai/voxl2/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
@@ -16,6 +16,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_pos_control start
+fw_lat_lon_control start
 airspeed_selector start

 #
@@ -28,6 +28,7 @@ fi
 fw_rate_control start vtol
 fw_att_control start vtol
 fw_pos_control start vtol
+fw_lat_lon_control start vtol
 fw_autotune_attitude_control start vtol

 # Start Land Detector
@@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -35,6 +35,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,6 +43,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -31,6 +31,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,6 +43,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,6 +51,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,6 +51,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -49,6 +49,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -33,6 +33,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,6 +51,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -31,6 +31,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -34,6 +34,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -33,6 +33,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -40,6 +40,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -41,6 +41,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -50,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -41,6 +41,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,6 +43,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,6 +44,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,6 +44,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -49,6 +49,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -34,6 +34,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -21,6 +21,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -25,6 +25,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -9,4 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
@@ -52,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -53,6 +53,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -56,6 +56,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -18,6 +18,7 @@ CONFIG_MODULES_ESC_BATTERY=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
@@ -5,6 +5,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -30,6 +30,7 @@ CONFIG_MODULES_EVENTS=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_GIMBAL=n
 CONFIG_MODULES_GYRO_CALIBRATION=n
@@ -60,6 +60,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -4,6 +4,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -3,6 +3,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -3,6 +3,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -58,6 +58,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -4,6 +4,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_COMMON_RC=y
@@ -3,6 +3,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -57,6 +57,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -3,6 +3,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
@@ -31,6 +31,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -20,6 +20,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
@@ -3,6 +3,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -33,6 +33,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -41,6 +41,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -91,6 +91,8 @@ set(msg_files
 	FollowTargetEstimator.msg
 	FollowTargetStatus.msg
 	FuelTankStatus.msg
+	FwLateralControlSetpoint.msg
+	FwLongitudinalControlSetpoint.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg
@@ -121,10 +123,12 @@ set(msg_files
 	LandingGearWheel.msg
 	LandingTargetInnovations.msg
 	LandingTargetPose.msg
+		LateralControlLimits.msg
 	LaunchDetectionStatus.msg
 	LedControl.msg
 	LoggerStatus.msg
 	LogMessage.msg
+	LongitudinalControlLimits.msg
 	MagnetometerBiasEstimate.msg
 	MagWorkerData.msg
 	ManualControlSwitches.msg
@@ -0,0 +1,12 @@
+uint64 timestamp
+
+# NOTE: At least one of course_setpoint, airspeed_reference_direction, or lateral_acceleration_setpoint must be finite
+float32 course_setpoint # NAN if not controlled directly, [-pi, pi]
+float32 airspeed_reference_direction    # angle of desired airspeed vector, NAN if not controlled directly, used as feedforward if course setpoint is finite, [-pi, pi],
+float32 lateral_acceleration_setpoint # NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_reference_direction is finite, [m/s^2]
+float32 roll_sp # TODO: remove, only for testing
+
+float32 heading_sp_runway_takeoff # [-pi, pi] heading setpoint for runway takeoff
+bool reset_integral # TODO: remove, resets rate controller integrals
+
+# TOPICS fw_lateral_control_setpoint fw_lateral_control_status
@@ -0,0 +1,11 @@
+uint64 timestamp
+
+
+# Note: If not both pitch_sp and throttle_sp are finite, then either altitude_setpoint or height_rate_setpoint must be finite
+float32 altitude_setpoint  # NAN if not controlled, MSL [m]
+float32 height_rate_setpoint # NAN if not controlled directly, used as feedforward if altitude_setpoint is finite [m/s]
+float32 equivalent_airspeed_setpoint # [m/s]
+float32 pitch_sp # NAN if not controlled, overrides total energy controller [rad]
+float32 thrust_sp # NAN if not controlled, overrides total energy controller [0,1]
+
+# TOPICS fw_longitudinal_control_setpoint fw_longitudinal_control_status
@@ -0,0 +1,3 @@
+uint64 timestamp
+
+float32 lateral_accel_max # maps 1:1 to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY, m/s^2
@@ -0,0 +1,15 @@
+uint64 timestamp
+
+float32 pitch_min   # [rad]
+float32 pitch_max   # [rad]
+float32 throttle_min # [0,1]
+float32 throttle_max # [0,1]
+float32 climb_rate_target # target climbrate used to change altitude, [m/s]
+float32 sink_rate_target # target sinkrate used to change altitude, [m/s]
+float32 equivalent_airspeed_min # [m/s]
+float32 equivalent_airspeed_max # [m/s]
+
+float32 speed_weight # [0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only
+
+bool enforce_low_height_condition # if true, total energy controller will use lower altitude control time constant
+bool disable_underspeed_protection
@@ -69,4 +69,4 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
 uint8 RTCM_MSG_USED_USED = 2
 uint8 rtcm_msg_used		# Indicates if the RTCM message was used successfully by the receiver

-# TOPICS sensor_gps vehicle_gps_position
+# TOPICS sensor_gps vehicle_gps_position platform_gps_position
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "AirspeedReferenceController.hpp"
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+using matrix::Vector2f;
+AirspeedReferenceController::AirspeedReferenceController()
+{
+	// Constructor
+}
+
+float AirspeedReferenceController::controlHeading(const float heading_sp, const float heading,
+		const float airspeed) const
+{
+
+	const Vector2f airspeed_vector = Vector2f{cosf(heading), sinf(heading)} * airspeed;
+	const Vector2f airspeed_sp_vector_unit = Vector2f{cosf(heading_sp), sinf(heading_sp)};
+
+	const float dot_air_vel_err = airspeed_vector.dot(airspeed_sp_vector_unit);
+	const float cross_air_vel_err = airspeed_vector.cross(airspeed_sp_vector_unit);
+
+	if (dot_air_vel_err < 0.0f) {
+		// hold max lateral acceleration command above 90 deg heading error
+		return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed);
+
+	} else {
+		// airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed
+		// for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed)
+		return p_gain_ * cross_air_vel_err;
+	}
+}
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file AirspeedRefController.hpp
+ *
+ * Original Author:  Thomas Stastny <tstastny@ethz.ch>
+ * Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
+ *
+ * * Notes:
+ * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts,
+ *   Otherwise the performance will suffer.
+ *
+ * Acknowledgements and References:
+ *
+ * The logic is mostly based on [1] and Paper III of [2].
+ * TODO: Concise, up to date documentation and stability analysis for the following
+ *       implementation.
+ *
+ * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of
+ *     Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference
+ *     on Intelligent Robots and Systems (IROS). 2019.
+ *     https://arxiv.org/pdf/1908.01381.pdf
+ * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small
+ *     Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020.
+ *     https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf
+ */
+
+#ifndef PX4_AIRSPEEDREFERENCECONTROLLER_HPP
+#define PX4_AIRSPEEDREFERENCECONTROLLER_HPP
+
+class AirspeedReferenceController
+{
+public:
+
+	AirspeedReferenceController();
+
+
+	float controlHeading(const float heading_sp, const float heading, const float airspeed) const;
+
+private:
+	float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s]
+};
+
+#endif //PX4_AIRSPEEDREFERENCECONTROLLER_HPP
@@ -32,8 +32,12 @@
 ############################################################################

 px4_add_library(npfg
-	npfg.cpp
-	npfg.hpp
+	DirectionalGuidance.cpp
+	CourseToAirspeedRefMapper.cpp
+	AirspeedReferenceController.cpp
+	DirectionalGuidance.hpp
+	CourseToAirspeedRefMapper.hpp
+	AirspeedReferenceController.hpp
 )

 target_link_libraries(npfg PRIVATE geo)
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "CourseToAirspeedRefMapper.hpp"
+using matrix::Vector2f;
+
+float
+CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel,
+		float airspeed_true) const
+{
+
+	Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)};
+	const float wind_cross_bearing = wind_vel.cross(bearing_vector);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vector);
+
+	const Vector2f air_vel_ref = refAirVelocity(wind_vel, bearing_vector, wind_cross_bearing,
+				     wind_dot_bearing, wind_vel.norm(), airspeed_true);
+
+	return atan2f(air_vel_ref(1), air_vel_ref(0));
+}
+
+matrix::Vector2f CourseToAirspeedRefMapper::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+		const float wind_cross_bearing, const float wind_dot_bearing,
+		const float wind_speed, float airspeed_true) const
+{
+	Vector2f air_vel_ref;
+
+	if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_true, wind_speed)) {
+		const float airsp_dot_bearing = projectAirspOnBearing(airspeed_true, wind_cross_bearing);
+		air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+	} else {
+		air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_true);
+	}
+
+	return air_vel_ref;
+}
+
+float CourseToAirspeedRefMapper::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
+{
+	// NOTE: wind_cross_bearing must be less than airspeed to use this function
+	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
+	// otherwise the return will be erroneous
+	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
+}
+
+int CourseToAirspeedRefMapper::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing,
+		const float airspeed, const float wind_speed) const
+{
+	return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed));
+}
+
+matrix::Vector2f
+CourseToAirspeedRefMapper::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+		const Vector2f &bearing_vec) const
+{
+	// essentially a 2D rotation with the speeds (magnitudes) baked in
+	return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1),
+			wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)};
+}
+
+matrix::Vector2f CourseToAirspeedRefMapper::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+		const float wind_speed, const float airspeed) const
+{
+	// NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function
+	// it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method
+	// otherwise the normalization of the air velocity vector could have a division by zero
+	Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel;
+	return air_vel_ref.normalized() * airspeed;
+}
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file CourseToAirspeedRefMapper.hpp
+ *
+ * Original Author:  Thomas Stastny <tstastny@ethz.ch>
+ * Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
+ *
+ * * Notes:
+ * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts,
+ *   Otherwise the performance will suffer.
+ *
+ * Acknowledgements and References:
+ *
+ * The logic is mostly based on [1] and Paper III of [2].
+ * TODO: Concise, up to date documentation and stability analysis for the following
+ *       implementation.
+ *
+ * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of
+ *     Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference
+ *     on Intelligent Robots and Systems (IROS). 2019.
+ *     https://arxiv.org/pdf/1908.01381.pdf
+ * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small
+ *     Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020.
+ *     https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf
+ */
+
+#ifndef PX4_COURSETOAIRSPEEDREFMAPPER_HPP
+#define PX4_COURSETOAIRSPEEDREFMAPPER_HPP
+
+
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+class CourseToAirspeedRefMapper
+{
+public:
+
+	CourseToAirspeedRefMapper() {};
+
+	~CourseToAirspeedRefMapper() = default;
+
+	float mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint,
+			const matrix::Vector2f &wind_vel, float airspeed_true) const;
+
+private:
+	/*
+	 * Determines a reference air velocity *without curvature compensation, but
+	 * including "optimal" true airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum true airspeed member variables must be set before using this method.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					const float wind_cross_bearing, const float wind_dot_bearing,
+					const float wind_speed, float airspeed_true) const;
+	/*
+	 * Projection of the air velocity vector onto the bearing line considering
+	 * a connected wind triangle.
+	 *
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @return Projection of air velocity vector on bearing vector [m/s]
+	 */
+	float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const;
+	/*
+	 * Check for binary bearing feasibility.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
+	 */
+	int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			      const float wind_speed) const;
+	/*
+	 * Air velocity solution for a given wind velocity and bearing vector assuming
+	 * a "high speed" (not backwards) solution in the excess wind case.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+					   const matrix::Vector2f &bearing_vec) const;
+	/*
+	 * Air velocity solution for an infeasible bearing.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					     const float wind_speed, const float airspeed) const;
+};
+
+#endif //PX4_COURSETOAIRSPEEDREFMAPPER_HPP
@@ -0,0 +1,316 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file DirectionalGuidance.cpp
+ */
+#include "DirectionalGuidance.hpp"
+using matrix::Vector2d;
+using matrix::Vector2f;
+
+DirectionalGuidance::DirectionalGuidance()
+{
+
+}
+
+DirectionalGuidanceOutput
+DirectionalGuidance::guideToPath(const 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();
+
+	const Vector2f air_vel = ground_vel - wind_vel;
+	const float airspeed = air_vel.norm();
+
+	const float wind_speed = wind_vel.norm();
+
+	const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
+	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);
+
+	// calculate the bearing feasibility on the track at the current closest point
+	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
+
+	const float track_error = fabsf(signed_track_error_);
+
+	// update control parameters considering upper and lower stability bounds (if enabled)
+	// must be called before trackErrorBound() as it updates time_const_
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
+				      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
+	_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);
+
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_);
+
+	// 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);
+
+	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
+	const float feas_combined = feas_ * feas_on_track_;
+	// lateral acceleration needed to stay on curved track (assuming no heading error)
+	const float lateral_accel_ff = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
+				       wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature) * feas_combined * track_proximity_;
+
+	return DirectionalGuidanceOutput{.course_setpoint = atan2f(bearing_vec_(1), bearing_vec_(0)),
+					 .lateral_acceleration_feedforward = lateral_accel_ff};
+}
+
+float DirectionalGuidance::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
+{
+	float period = period_;
+	const float air_turn_rate = fabsf(path_curvature * airspeed);
+	const float wind_factor = windFactor(airspeed, wind_speed);
+
+	if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) {
+		// lower bound for period not considering path curvature
+		const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_;
+
+		// lower bound for period *considering path curvature
+		float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_;
+
+		// calculate the time constant and track error bound considering the zero
+		// curvature, lower-bounded period and subsequently recalculate the normalized
+		// track error
+		const float time_const = timeConst(period_lb_zero_curvature, damping_);
+		const float track_error_bound = trackErrorBound(ground_speed, time_const);
+		const float normalized_track_error = normalizedTrackError(track_error, track_error_bound);
+
+		// calculate nominal track proximity with lower bounded time constant
+		// (only a numerical solution can find corresponding track proximity
+		// and adapted gains simultaneously)
+		const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+		const float track_proximity = trackProximity(look_ahead_ang);
+
+		// linearly ramp in curvature dependent lower bound with track proximity
+		period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature;
+
+		// lower bounded period
+		period = math::max(period_lb, period);
+
+		// only allow upper bounding ONLY if lower bounding is enabled (is otherwise
+		// dangerous to allow period decrements without stability checks).
+		// NOTE: if the roll time constant is not accurately known, lower-bound
+		// checks may be too optimistic and reducing the period can still destabilize
+		// the system! enable this feature at your own risk.
+		if (en_period_ub_) {
+
+			const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track);
+
+			if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) {
+				// NOTE: if the roll time constant is not accurately known, reducing
+				// the period here can destabilize the system!
+				// enable this feature at your own risk!
+
+				// upper bound the period (for track keeping stability), prefer lower bound if violated
+				const float period_adapted = math::max(period_lb, period_ub);
+
+				// transition from the nominal period to the adapted period as we get
+				// closer to the track
+				period = period_adapted * track_proximity + (1.0f - track_proximity) * period;
+			}
+		}
+	}
+
+	return period;
+}
+
+float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const
+{
+	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
+}
+
+
+
+float DirectionalGuidance::windFactor(const float airspeed, const float wind_speed) const
+{
+	// See [TODO: include citation] for definition/elaboration of this approximation.
+	if (wind_speed > airspeed || airspeed < NPFG_EPSILON) {
+		return 2.0f;
+
+	} else {
+		return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
+	}
+}
+
+float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor,
+		const float feas_on_track) const
+{
+	if (air_turn_rate * wind_factor > NPFG_EPSILON) {
+		// multiply air turn rate by feasibility on track to zero out when we anyway
+		// should not consider the curvature
+		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
+	}
+
+	return INFINITY;
+}
+
+float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const float wind_factor,
+		const float feas_on_track) const
+{
+	// this method considers a "conservative" lower period bound, i.e. a constant
+	// worst case bound for any wind ratio, airspeed, and path curvature
+
+	// the lower bound for zero curvature and no wind OR damping ratio < 0.5
+	const float period_lb = M_PI_F * roll_time_const_ / damping_;
+
+	if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) {
+		return period_lb;
+
+	} else {
+		// the lower bound for tracking a curved path in wind with damping ratio > 0.5
+		const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_;
+
+		// blend the two together as the bearing on track becomes less feasible
+		return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb;
+	}
+}
+
+float DirectionalGuidance::trackProximity(const float look_ahead_ang) const
+{
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+	return sin_look_ahead_ang * sin_look_ahead_ang;
+}
+
+float DirectionalGuidance::trackErrorBound(const float ground_speed, const float time_const) const
+{
+	if (ground_speed > 1.0f) {
+		return ground_speed * time_const;
+
+	} else {
+		// limit bound to some minimum ground speed to avoid singularities in track
+		// error normalization. the following equation assumes ground speed minimum = 1.0
+		return 0.5f * time_const * (ground_speed * ground_speed + 1.0f);
+	}
+}
+
+float DirectionalGuidance::timeConst(const float period, const float damping) const
+{
+	return period * damping;
+}
+
+float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) const
+{
+	return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
+}
+
+
+matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
+		const float signed_track_error) const
+{
+	const float cos_look_ahead_ang = cosf(look_ahead_ang);
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+
+	Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn
+	Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal;
+
+	return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent;
+}
+
+float
+DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+					const float wind_speed) const
+{
+	if (wind_dot_bearing < 0.0f) {
+		wind_cross_bearing = wind_speed;
+
+	} else {
+		wind_cross_bearing = fabsf(wind_cross_bearing);
+	}
+
+	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f));
+	return sin_arg * sin_arg;
+}
+
+float DirectionalGuidance::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
+		const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
+		const float wind_speed, const float signed_track_error,
+		const float path_curvature) const
+{
+	// NOTE: all calculations within this function take place at the closet point
+	// on the path, as if the aircraft were already tracking the given path at
+	// this point with zero angular error. this allows us to evaluate curvature
+	// induced requirements for lateral acceleration incrementation and ramp them
+	// in with the track proximity and further consider the bearing feasibility
+	// in excess wind conditions (this is done external to this method).
+
+	// path frame curvature is the instantaneous curvature at our current distance
+	// from the actual path (considering e.g. concentric circles emanating outward/inward)
+	const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error,
+					   fabsf(path_curvature) * MIN_RADIUS);
+
+	// limit tangent ground speed to along track (forward moving) direction
+	const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f);
+
+	const float path_frame_rate = path_frame_curvature * tangent_ground_speed;
+
+	// speed ratio = projection of ground vel on track / projection of air velocity
+	// on track
+	const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt),
+				   NPFG_EPSILON));
+
+	// note the use of airspeed * speed_ratio as oppose to ground_speed^2 here --
+	// the prior considers that we command lateral acceleration in the air mass
+	// relative frame while the latter does not
+	return airspeed * speed_ratio * path_frame_rate;
+}
+
+float DirectionalGuidance::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
+{
+	// NOTE: wind_cross_bearing must be less than airspeed to use this function
+	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
+	// otherwise the return will be erroneous
+	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
+}
+
+float DirectionalGuidance::switchDistance(float wp_radius) const
+{
+	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- * Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,13 +32,12 @@
 ****************************************************************************/

 /*
- * @file npfg.hpp
- * Implementation of a lateral-directional nonlinear path following guidance
- * law with excess wind handling. Commands lateral acceleration and airspeed.
+ * @file DirectionalGuidance.hpp
 *
- * @author Thomas Stastny <tstastny@ethz.ch>
+ * Original Author:  Thomas Stastny <tstastny@ethz.ch>
+ * Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
 *
- * Notes:
+ * * Notes:
 * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts,
 *   Otherwise the performance will suffer.
 *
@@ -57,50 +56,26 @@
 *     https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf
 */

-#ifndef NPFG_H_
-#define NPFG_H_
-
+#ifndef PX4_DIRECTIONALGUIDANCE_HPP
+#define PX4_DIRECTIONALGUIDANCE_HPP
 #include <matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>

-#include <uORB/topics/vehicle_local_position.h>
+struct DirectionalGuidanceOutput {
+	float course_setpoint{NAN};
+	float lateral_acceleration_feedforward{NAN};
+};

-/*
- * NPFG
- * Lateral-directional nonlinear path following guidance logic with excess wind handling
- */
-class NPFG
+class DirectionalGuidance
 {
-
 public:
-	/**
-	 * @brief Can run
-	 *
-	 * Evaluation if all the necessary information are available such that npfg can produce meaningful results.
-	 *
-	 * @param[in] local_pos is the current vehicle local position uorb message
-	 * @param[in] is_wind_valid flag if the wind estimation is valid
-	 * @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions.
-	 */

-	float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
-	/*
-	 * Computes the lateral acceleration and true 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);
+	DirectionalGuidance();

+	DirectionalGuidanceOutput 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].
 	 */
@@ -122,42 +97,6 @@ public:
 	 */
 	void enablePeriodUB(const bool en) { en_period_ub_ = en; }

-	/*
-	 * Enable minimum forward ground speed maintenance logic.
-	 */
-	void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; }
-
-	/*
-	 * Enable track keeping logic in excess wind conditions.
-	 */
-	void enableTrackKeeping(const bool en) { en_track_keeping_ = en; }
-
-	/*
-	 * Enable wind excess regulation. Disabling this param disables all airspeed
-	 * reference incrementaion (airspeed reference will always be nominal).
-	 */
-	void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; }
-
-	/*
-	 * Set the minimum allowed forward ground speed [m/s].
-	 */
-	void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = math::max(min_gsp, 0.0f); }
-
-	/*
-	 * Set the maximum value of the minimum forward ground speed command for track
-	 * keeping (occurs at the track error boundary) [m/s].
-	 */
-	void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); }
-
-	/*
-	 * Set the nominal airspeed reference (true airspeed) [m/s].
-	 */
-	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }
-
-	/*
-	 * Set the maximum airspeed reference (true airspeed) [m/s].
-	 */
-	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }

 	/*
 	 * Set the autopilot roll response time constant [s].
@@ -165,88 +104,14 @@ public:
 	void setRollTimeConst(float tc) { roll_time_const_ = tc; }

 	/*
-	 * Set the switch distance multiplier.
-	 */
+	* Set the switch distance multiplier.
+	*/
 	void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); }

 	/*
 	 * Set the period safety factor.
 	 */
 	void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); }
-
-	/*
-	 * @return Controller proportional gain [rad/s]
-	 */
-	float getPGain() const { return p_gain_; }
-
-	/*
-	 * @return Controller time constant [s]
-	 */
-	float getTimeConst() const { return time_const_; }
-
-	/*
-	 * @return Adapted controller period [s]
-	 */
-	float getAdaptedPeriod() const { return adapted_period_; }
-
-	/*
-	 * @return Track error boundary [m]
-	 */
-	float getTrackErrorBound() const { return track_error_bound_; }
-
-	/*
-	 * @return Signed track error [m]
-	 */
-	float getTrackError() const { return signed_track_error_; }
-
-	/*
-	 * @return Airspeed reference [m/s]
-	 */
-	float getAirspeedRef() const { return airspeed_ref_; }
-
-	/*
-	 * @return Heading angle reference [rad]
-	 */
-	float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); }
-
-	/*
-	 * @return Bearing angle [rad]
-	 */
-	float getBearing() const { return atan2f(bearing_vec_(1), bearing_vec_(0)); }
-
-	/*
-	 * @return Lateral acceleration command [m/s^2]
-	 */
-	float getLateralAccel() const { return lateral_accel_; }
-
-	/*
-	 * @return Feed-forward lateral acceleration command increment for tracking
-	 * path curvature [m/s^2]
-	 */
-	float getLateralAccelFF() const { return lateral_accel_ff_; }
-
-	/*
-	 * @return Bearing feasibility [0, 1]
-	 */
-	float getBearingFeas() const { return feas_; }
-
-	/*
-	 * @return On-track bearing feasibility [0, 1]
-	 */
-	float getOnTrackBearingFeas() const { return feas_on_track_; }
-
-	/*
-	 * @return Minimum forward ground speed reference [m/s]
-	 */
-	float getMinGroundSpeedRef() const { return min_ground_speed_ref_; }
-
-	/*
-	 * [Copied directly from ECL_L1_Pos_Controller]
-	 *
-	 * Set the maximum roll angle output in radians
-	 */
-	void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; }
-
 	/*
 	 * [Copied directly from ECL_L1_Pos_Controller]
 	 *
@@ -259,38 +124,14 @@ public:
 	 */
 	float switchDistance(float wp_radius) const;

-	/*
-	 * [Copied directly from ECL_L1_Pos_Controller]
-	 *
-	 * Get roll angle setpoint for fixed wing.
-	 *
-	 * @return Roll angle (in NED frame)
-	 */
-	float getRollSetpoint() { return roll_setpoint_; }
-
 private:
-	static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
-	static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
-	static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
-	static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
-
 	static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon
 	static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]
-	static constexpr float NTE_FRACTION = 0.5f; // normalized track error fraction (must be > 0)
-	// ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached
 	static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0)
-	// ^the size of the feasibility transition region at cross wind angle >= 90 deg.
-	//  This must be non-zero to avoid jumpy airspeed incrementation while using wind
-	//  excess handling logic. Similarly used as buffer region around zero airspeed.
-
-	/*
-	 * tuning
-	 */

 	float period_{10.0f}; // nominal (desired) period -- user defined [s]
 	float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined
-	float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s]
-	float time_const_{7.071f}; // time constant (computed from period_ and damping_) [s]
+	float _time_const{7.0f}; // time constant (computed from period_ and damping_) [s]
 	float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s]

 	/*
@@ -300,17 +141,9 @@ private:
 	// guidance options
 	bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period
 	bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled)
-	bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed
-	bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides
-	bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ...
-	// ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference

 	// guidance settings
-	float airspeed_nom_{15.0f}; // nominal (desired) true airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
-	float airspeed_max_{20.0f}; // maximum true airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
 	float roll_time_const_{0.0f}; // autopilot roll response time constant [s]
-	float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s]
-	float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s]

 	// guidance parameters
 	float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance
@@ -318,12 +151,8 @@ private:
 	float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound

 	/*
-	 * internal guidance states
-	 */
-
-	// speeds
-	float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s]
-	float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s]
+	* internal guidance states
+	*/

 	//bearing feasibility
 	float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible)
@@ -332,27 +161,22 @@ private:
 	// track proximity
 	float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m]
 	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
 	float signed_track_error_{0.0f}; // signed track error [m]
 	matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector

-	/*
-	 * guidance outputs
-	 */
-
-	float airspeed_ref_{15.0f}; // true airspeed reference [m/s]
-	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s]
-	float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2]
-	float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2]

 	/*
-	 * ECL_L1_Pos_Controller functionality
+	 * Cacluates a continuous representation of the bearing feasibility from [0,1].
+	 * 0 = infeasible, 1 = fully feasible, partial feasibility in between.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return bearing feasibility
 	 */
-
-	float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
-	float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
-
+	float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+				 const float wind_speed) const;
 	/*
 	 * Adapts the controller period considering user defined inputs, current flight
 	 * condition, path properties, and stability bounds.
@@ -371,7 +195,6 @@ private:
 	float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
 			  const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel,
 			  const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const;
-
 	/*
 	 * Returns normalized (unitless) and constrained track error [0,1].
 	 *
@@ -435,17 +258,6 @@ private:
 	 */
 	float trackErrorBound(const float ground_speed, const float time_const) const;

-	/*
-	 * Calculates the required controller proportional gain to achieve the desired
-	 * system period and damping ratio. NOTE: actual period and damping will vary
-	 * when following paths with curvature in wind.
-	 *
-	 * @param[in] period Desired system period [s]
-	 * @param[in] damping Desired system damping ratio
-	 * @return Proportional gain [rad/s]
-	 */
-	float pGain(const float period, const float damping) const;
-
 	/*
 	 * Calculates the required controller time constant to achieve the desired
 	 * system period and damping ratio. NOTE: actual period and damping will vary
@@ -465,7 +277,6 @@ private:
 	 * @return Look ahead angle [rad]
 	 */
 	float lookAheadAngle(const float normalized_track_error) const;
-
 	/*
 	 * Calculates the bearing vector and track proximity transitioning variable
 	 * from the look-ahead angle mapping.
@@ -481,33 +292,6 @@ private:
 	matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang,
 				    const float signed_track_error) const;

-	/*
-	 * Calculates the minimum forward ground speed demand for minimum forward
-	 * ground speed maintanence as well as track keeping logic.
-	 *
-	 * @param[in] normalized_track_error Normalized track error (track error / track error boundary)
-	 * @param[in] feas Bearing feasibility
-	 * @return Minimum forward ground speed demand [m/s]
-	 */
-	float minGroundSpeed(const float normalized_track_error, const float feas);
-
-	/*
-	 * Determines a reference air velocity *without curvature compensation, but
-	 * including "optimal" true airspeed reference compensation in excess wind conditions.
-	 * Nominal and maximum true airspeed member variables must be set before using this method.
-	 *
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] bearing_vec Bearing vector
-	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
-	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] wind_speed Wind speed [m/s]
-	 * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s]
-	 * @return Air velocity vector [m/s]
-	 */
-	matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
-					const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
-					const float min_ground_speed) const;
-
 	/*
 	 * Projection of the air velocity vector onto the bearing line considering
 	 * a connected wind triangle.
@@ -517,58 +301,6 @@ private:
 	 * @return Projection of air velocity vector on bearing vector [m/s]
 	 */
 	float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const;
-
-	/*
-	 * Check for binary bearing feasibility.
-	 *
-	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
-	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle true airspeed [m/s]
-	 * @param[in] wind_speed Wind speed [m/s]
-	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
-	 */
-	int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
-			      const float wind_speed) const;
-
-	/*
-	 * Air velocity solution for a given wind velocity and bearing vector assuming
-	 * a "high speed" (not backwards) solution in the excess wind case.
-	 *
-	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
-	 * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s]
-	 * @param[in] bearing_vec Bearing vector
-	 * @return Air velocity vector [m/s]
-	 */
-	matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
-					   const matrix::Vector2f &bearing_vec) const;
-
-
-	/*
-	 * Air velocity solution for an infeasible bearing.
-	 *
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] bearing_vec Bearing vector
-	 * @param[in] wind_speed Wind speed [m/s]
-	 * @param[in] airspeed Vehicle true airspeed [m/s]
-	 * @return Air velocity vector [m/s]
-	 */
-	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
-					     const float wind_speed, const float airspeed) const;
-
-
-	/*
-	 * Cacluates a continuous representation of the bearing feasibility from [0,1].
-	 * 0 = infeasible, 1 = fully feasible, partial feasibility in between.
-	 *
-	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
-	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle true airspeed [m/s]
-	 * @param[in] wind_speed Wind speed [m/s]
-	 * @return bearing feasibility
-	 */
-	float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
-				 const float wind_speed) const;
-
 	/*
 	 * Calculates an additional feed-forward lateral acceleration demand considering
 	 * the path curvature.
@@ -588,29 +320,6 @@ private:
 			     const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
 			     const float wind_speed, const float signed_track_error, const float path_curvature) const;

-	/*
-	 * Calculates a lateral acceleration demand from the heading error.
-	 * (does not consider path curvature)
-	 *
-	 * @param[in] air_vel Vechile air velocity vector [m/s]
-	 * @param[in] air_vel_ref Reference air velocity vector [m/s]
-	 * @param[in] airspeed Vehicle true airspeed [m/s]
-	 * @return Lateral acceleration demand [m/s^2]
-	 */
-	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
-			   const float airspeed) const;
+};

-	/*******************************************************************************
-	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
-	 */
-
-	/**
-	 * [Copied directly from ECL_L1_Pos_Controller]
-	 *
-	 * Update roll angle setpoint. This will also apply slew rate limits if set.
-	 */
-	void updateRollSetpoint();
-
-}; // class NPFG
-
-#endif // NPFG_H_
+#endif //PX4_DIRECTIONALGUIDANCE_HPP
@@ -1,508 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file npfg.cpp
- * Implementation of a lateral-directional nonlinear path following guidance
- * law with excess wind handling. Commands lateral acceleration and airspeed.
- *
- * Authors and acknowledgements in header.
- */
-
-#include "npfg.hpp"
-#include <lib/geo/geo.h>
-#include <px4_platform_common/defines.h>
-#include <float.h>
-
-using matrix::Vector2d;
-using matrix::Vector2f;
-
-float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
-{
-	if (is_wind_valid) {
-		// If we have a valid wind estimate, npfg is able to handle all degenerated cases
-		return 1.f;
-	}
-
-	// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
-	// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
-	const Vector2f ground_vel(local_pos.vx, local_pos.vy);
-	const float ground_speed(ground_vel.norm());
-	const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
-					    local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
-					    0.f, 1.f));
-
-	// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
-	const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
-	const Vector2f ground_vel_norm(ground_vel.normalized());
-	const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
-					  COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
-							  COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
-					  0.f, 1.f));
-
-	return flying_forward_factor * low_ground_speed_factor;
-}
-
-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();
-
-	const Vector2f air_vel = ground_vel - wind_vel;
-	const float airspeed = air_vel.norm();
-
-	const float wind_speed = wind_vel.norm();
-
-	const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
-	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);
-
-	// calculate the bearing feasibility on the track at the current closest point
-	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
-
-	const float track_error = fabsf(signed_track_error_);
-
-	// update control parameters considering upper and lower stability bounds (if enabled)
-	// must be called before trackErrorBound() as it updates time_const_
-	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
-				      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
-	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);
-
-	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_);
-
-	// 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);
-
-	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
-	const float feas_combined = feas_ * feas_on_track_;
-
-	min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined);
-
-	// 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
-	const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed);
-
-	// lateral acceleration needed to stay on curved track (assuming no heading error)
-	lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
-					   wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature);
-
-	// total lateral acceleration to drive aircaft towards track as well as account
-	// for path curvature. The full effect of the feed-forward acceleration is smoothly
-	// 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
-
-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
-{
-	float period = period_;
-	const float air_turn_rate = fabsf(path_curvature * airspeed);
-	const float wind_factor = windFactor(airspeed, wind_speed);
-
-	if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) {
-		// lower bound for period not considering path curvature
-		const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_;
-
-		// lower bound for period *considering path curvature
-		float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_;
-
-		// calculate the time constant and track error bound considering the zero
-		// curvature, lower-bounded period and subsequently recalculate the normalized
-		// track error
-		const float time_const = timeConst(period_lb_zero_curvature, damping_);
-		const float track_error_bound = trackErrorBound(ground_speed, time_const);
-		const float normalized_track_error = normalizedTrackError(track_error, track_error_bound);
-
-		// calculate nominal track proximity with lower bounded time constant
-		// (only a numerical solution can find corresponding track proximity
-		// and adapted gains simultaneously)
-		const float look_ahead_ang = lookAheadAngle(normalized_track_error);
-		const float track_proximity = trackProximity(look_ahead_ang);
-
-		// linearly ramp in curvature dependent lower bound with track proximity
-		period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature;
-
-		// lower bounded period
-		period = math::max(period_lb, period);
-
-		// only allow upper bounding ONLY if lower bounding is enabled (is otherwise
-		// dangerous to allow period decrements without stability checks).
-		// NOTE: if the roll time constant is not accurately known, lower-bound
-		// checks may be too optimistic and reducing the period can still destabilize
-		// the system! enable this feature at your own risk.
-		if (en_period_ub_) {
-
-			const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track);
-
-			if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) {
-				// NOTE: if the roll time constant is not accurately known, reducing
-				// the period here can destabilize the system!
-				// enable this feature at your own risk!
-
-				// upper bound the period (for track keeping stability), prefer lower bound if violated
-				const float period_adapted = math::max(period_lb, period_ub);
-
-				// transition from the nominal period to the adapted period as we get
-				// closer to the track
-				period = period_adapted * track_proximity + (1.0f - track_proximity) * period;
-			}
-		}
-	}
-
-	return period;
-} // adaptPeriod
-
-float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const
-{
-	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
-}
-
-float NPFG::windFactor(const float airspeed, const float wind_speed) const
-{
-	// See [TODO: include citation] for definition/elaboration of this approximation.
-	if (wind_speed > airspeed || airspeed < NPFG_EPSILON) {
-		return 2.0f;
-
-	} else {
-		return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
-	}
-} // windFactor
-
-float NPFG::periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
-{
-	if (air_turn_rate * wind_factor > NPFG_EPSILON) {
-		// multiply air turn rate by feasibility on track to zero out when we anyway
-		// should not consider the curvature
-		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
-	}
-
-	return INFINITY;
-} // periodUB
-
-float NPFG::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
-{
-	// this method considers a "conservative" lower period bound, i.e. a constant
-	// worst case bound for any wind ratio, airspeed, and path curvature
-
-	// the lower bound for zero curvature and no wind OR damping ratio < 0.5
-	const float period_lb = M_PI_F * roll_time_const_ / damping_;
-
-	if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) {
-		return period_lb;
-
-	} else {
-		// the lower bound for tracking a curved path in wind with damping ratio > 0.5
-		const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_;
-
-		// blend the two together as the bearing on track becomes less feasible
-		return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb;
-	}
-} // periodLB
-
-float NPFG::trackProximity(const float look_ahead_ang) const
-{
-	const float sin_look_ahead_ang = sinf(look_ahead_ang);
-	return sin_look_ahead_ang * sin_look_ahead_ang;
-} // trackProximity
-
-float NPFG::trackErrorBound(const float ground_speed, const float time_const) const
-{
-	if (ground_speed > 1.0f) {
-		return ground_speed * time_const;
-
-	} else {
-		// limit bound to some minimum ground speed to avoid singularities in track
-		// error normalization. the following equation assumes ground speed minimum = 1.0
-		return 0.5f * time_const * (ground_speed * ground_speed + 1.0f);
-	}
-} // trackErrorBound
-
-float NPFG::pGain(const float period, const float damping) const
-{
-	return 4.0f * M_PI_F * damping / period;
-} // pGain
-
-float NPFG::timeConst(const float period, const float damping) const
-{
-	return period * damping;
-} // timeConst
-
-float NPFG::lookAheadAngle(const float normalized_track_error) const
-{
-	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,
-			  const float signed_track_error) const
-{
-	const float cos_look_ahead_ang = cosf(look_ahead_ang);
-	const float sin_look_ahead_ang = sinf(look_ahead_ang);
-
-	Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn
-	Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal;
-
-	return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent;
-} // bearingVec
-
-float NPFG::minGroundSpeed(const float normalized_track_error, const float feas)
-{
-	// minimum ground speed demand from track keeping logic
-	min_gsp_track_keeping_ = 0.0f;
-
-	if (en_track_keeping_ && en_wind_excess_regulation_) {
-		// zero out track keeping speed increment when bearing is feasible
-		// maximum track keeping speed increment is applied until we are within
-		// a user defined fraction of the normalized track error
-		min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain(
-						 normalized_track_error / NTE_FRACTION, 0.0f,
-						 1.0f);
-	}
-
-	// minimum ground speed demand from minimum forward ground speed user setting
-	float min_gsp_desired = 0.0f;
-
-	if (en_min_ground_speed_ && en_wind_excess_regulation_) {
-		min_gsp_desired = min_gsp_desired_;
-	}
-
-	return math::max(min_gsp_track_keeping_, min_gsp_desired);
-} // minGroundSpeed
-
-Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
-			      const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
-			      const float min_ground_speed) const
-{
-	Vector2f air_vel_ref;
-
-	if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) {
-		// minimum ground speed and/or track keeping
-
-		// airspeed required to achieve minimum ground speed along bearing vector
-		const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) +
-						 wind_cross_bearing * wind_cross_bearing);
-
-		if (airspeed_min > airspeed_max_) {
-			if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) {
-				// we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed
-				const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing);
-				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
-
-			} else {
-				// bearing is maximally infeasible, employ mitigation law
-				air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_);
-			}
-
-		} else if (airspeed_min > airspeed_nom_) {
-			// the minimum ground speed is achievable within the nom - max airspeed range
-			// solve wind triangle with for air velocity reference with minimum airspeed
-			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing);
-			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
-
-		} else {
-			// the minimum required airspeed is less than nominal, so we can track the bearing and minimum
-			// ground speed with our nominal airspeed reference
-			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
-			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
-		}
-
-	} else {
-		// wind excess regulation and/or mitigation
-
-		if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) {
-			// bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed
-			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
-			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
-
-		} else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)
-			   && en_wind_excess_regulation_) {
-			// bearing is maximally feasible
-			if (wind_dot_bearing <= 0.0f) {
-				// we only increment the airspeed to regulate, but not overcome, excess wind
-				// NOTE: in the terminal condition, this will result in a zero ground velocity configuration
-				air_vel_ref = wind_vel;
-
-			} else {
-				// the bearing is achievable within the nom - max airspeed range
-				// solve wind triangle with for air velocity reference with minimum airspeed
-				const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage
-				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
-			}
-
-		} else {
-			// bearing is maximally infeasible, employ mitigation law
-			const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_;
-			air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input);
-		}
-	}
-
-	return air_vel_ref;
-} // refAirVelocity
-
-float NPFG::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
-{
-	// NOTE: wind_cross_bearing must be less than airspeed to use this function
-	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
-	// otherwise the return will be erroneous
-	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
-} // projectAirspOnBearing
-
-int NPFG::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
-			    const float wind_speed) const
-{
-	return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed));
-} // bearingIsFeasible
-
-Vector2f NPFG::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
-				 const Vector2f &bearing_vec) const
-{
-	// essentially a 2D rotation with the speeds (magnitudes) baked in
-	return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1),
-			wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)};
-} // solveWindTriangle
-
-Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, const float wind_speed,
-				   const float airspeed) const
-{
-	// NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function
-	// it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method
-	// otherwise the normalization of the air velocity vector could have a division by zero
-	Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel;
-	return air_vel_ref.normalized() * airspeed;
-} // infeasibleAirVelRef
-
-float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
-			       const float wind_speed) const
-{
-	if (wind_dot_bearing < 0.0f) {
-		wind_cross_bearing = wind_speed;
-
-	} else {
-		wind_cross_bearing = fabsf(wind_cross_bearing);
-	}
-
-	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f));
-	return sin_arg * sin_arg;
-} // bearingFeasibility
-
-float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
-			   const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
-			   const float wind_speed, const float signed_track_error, const float path_curvature) const
-{
-	// NOTE: all calculations within this function take place at the closet point
-	// on the path, as if the aircraft were already tracking the given path at
-	// this point with zero angular error. this allows us to evaluate curvature
-	// induced requirements for lateral acceleration incrementation and ramp them
-	// in with the track proximity and further consider the bearing feasibility
-	// in excess wind conditions (this is done external to this method).
-
-	// path frame curvature is the instantaneous curvature at our current distance
-	// from the actual path (considering e.g. concentric circles emanating outward/inward)
-	const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error,
-					   fabsf(path_curvature) * MIN_RADIUS);
-
-	// limit tangent ground speed to along track (forward moving) direction
-	const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f);
-
-	const float path_frame_rate = path_frame_curvature * tangent_ground_speed;
-
-	// speed ratio = projection of ground vel on track / projection of air velocity
-	// on track
-	const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt),
-				   NPFG_EPSILON));
-
-	// note the use of airspeed * speed_ratio as oppose to ground_speed^2 here --
-	// the prior considers that we command lateral acceleration in the air mass
-	// relative frame while the latter does not
-	return airspeed * speed_ratio * path_frame_rate;
-} // lateralAccelFF
-
-float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const
-{
-	// lateral acceleration demand only from the heading error
-
-	const float dot_air_vel_err = air_vel.dot(air_vel_ref);
-	const float cross_air_vel_err = air_vel.cross(air_vel_ref);
-
-	if (dot_air_vel_err < 0.0f) {
-		// hold max lateral acceleration command above 90 deg heading error
-		return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed);
-
-	} else {
-		// airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed
-		// for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed)
-		return p_gain_ * cross_air_vel_err / airspeed_ref_;
-	}
-} // lateralAccel
-
-float NPFG::switchDistance(float wp_radius) const
-{
-	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
-} // switchDistance
-
-void NPFG::updateRollSetpoint()
-{
-	float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G);
-	roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_);
-
-	if (PX4_ISFINITE(roll_new)) {
-		roll_setpoint_ = roll_new;
-	}
-} // updateRollSetpoint
@@ -0,0 +1,15 @@
+set(CONTROL_DEPENDENCIES
+        npfg
+        tecs
+)
+
+
+px4_add_module(
+        MODULE modules__fw_lateral_longitudinal_control
+        MAIN fw_lat_lon_control
+        SRCS
+        FwLateralLongitudinalControl.cpp
+        FwLateralLongitudinalControl.hpp
+        DEPENDS
+        ${CONTROL_DEPENDENCIES}
+)
@@ -0,0 +1,764 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "FwLateralLongitudinalControl.hpp"
+#include <px4_platform_common/events.h>
+
+using math::constrain;
+using math::max;
+using math::radians;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector2f;
+
+// [m/s] maximum reference altitude rate threshhold
+static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
+// [s] slew rate with which we change altitude time constant
+static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
+
+static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
+static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
+static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
+static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
+
+// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
+static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
+
+// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
+static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
+
+
+FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_attitude_sp_pub(is_vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+}
+
+FwLateralLongitudinalControl::~FwLateralLongitudinalControl()
+{
+	perf_free(_loop_perf);
+}
+void
+FwLateralLongitudinalControl::parameters_update()
+{
+	updateParams();
+	_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
+	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
+	_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
+	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
+	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
+	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
+	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
+	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
+	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
+	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
+	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
+	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
+	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
+	_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
+	_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
+	_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
+	_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
+	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
+	_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
+	_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
+	_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
+
+	_performance_model.runSanityChecks();
+
+	_performance_model.updateParameters();
+	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
+
+	_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
+	_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
+}
+
+void FwLateralLongitudinalControl::Run()
+{
+	if (should_exit()) {
+		_local_pos_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	/* only run controller if position changed */
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		parameters_update();
+	}
+
+	if (_local_pos_sub.update(&_local_pos)) {
+
+		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f,
+					       0.001f, 0.1f);
+		_last_time_loop_ran = _local_pos.timestamp;
+
+		updateControlLimits();
+
+		_tecs.set_speed_weight(_long_limits.speed_weight);
+		updateTECSAltitudeTimeConstant(checkLowHeightConditions()
+					       || _long_limits.enforce_low_height_condition, control_interval);
+		_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
+
+		if (_vehicle_air_data_sub.updated()) {
+			_air_density = PX4_ISFINITE(_vehicle_air_data_sub.get().rho) ? _vehicle_air_data_sub.get().rho : _air_density;
+			_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
+			_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
+		}
+
+		if (_vehicle_landed_sub.updated()) {
+			vehicle_land_detected_s landed{};
+			_vehicle_landed_sub.copy(&landed);
+			_landed = landed.landed;
+		}
+
+		_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
+
+		_vehicle_status_sub.update();
+		_control_mode_sub.update();
+		update_control_state();
+
+		if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
+		    && _local_pos.z_reset_counter != _z_reset_counter) {
+			if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
+				// make TECS accept step in altitude and demanded altitude
+				_tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate);
+			}
+		}
+
+		const bool should_run = _control_mode_sub.get().flag_control_position_enabled ||
+					_control_mode_sub.get().flag_control_velocity_enabled ||
+					_control_mode_sub.get().flag_control_acceleration_enabled ||
+					_control_mode_sub.get().flag_control_altitude_enabled ||
+					_control_mode_sub.get().flag_control_climb_rate_enabled;
+
+		if (should_run) {
+			float pitch_sp{NAN};
+			float throttle_sp{NAN};
+
+			if (_fw_longitudinal_ctrl_sub.updated()) {
+				_fw_longitudinal_ctrl_sub.copy(&_long_control_sp);
+			}
+
+			tecs_update_pitch_throttle(control_interval, _long_control_sp.altitude_setpoint,
+						   PX4_ISFINITE(_long_control_sp.equivalent_airspeed_setpoint)
+						   ? _long_control_sp.equivalent_airspeed_setpoint :
+						   _performance_model.getCalibratedTrimAirspeed(),
+						   _long_limits.pitch_min,
+						   _long_limits.pitch_max,
+						   _long_limits.throttle_min,
+						   _long_limits.throttle_max,
+						   _long_limits.sink_rate_target,
+						   _long_limits.climb_rate_target,
+						   _long_limits.disable_underspeed_protection,
+						   _long_control_sp.height_rate_setpoint
+						  );
+			pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_sp) ? _long_control_sp.pitch_sp : _tecs.get_pitch_setpoint();
+			throttle_sp = PX4_ISFINITE(_long_control_sp.thrust_sp) ? _long_control_sp.thrust_sp : _tecs.get_throttle_setpoint();
+
+			fw_longitudinal_control_setpoint_s longitudinal_control_status {
+				.timestamp = hrt_absolute_time(),
+				.altitude_setpoint = _long_control_sp.altitude_setpoint,
+				.height_rate_setpoint = _tecs.getStatus().control.altitude_rate_control,
+				.equivalent_airspeed_setpoint = _tecs.getStatus().true_airspeed_sp / _long_control_state.eas2tas,
+			};
+
+			_longitudinal_ctrl_status_pub.publish(longitudinal_control_status);
+
+			float roll_sp {NAN};
+			float yaw_sp {NAN};
+
+			if (_fw_lateral_ctrl_sub.updated()) {
+				_fw_lateral_ctrl_sub.copy(&_lat_control_sp);
+			}
+
+			float airspeed_reference_direction{NAN};
+			float lateral_accel_sp {NAN};
+
+			if (PX4_ISFINITE(_lat_control_sp.course_setpoint)) {
+				airspeed_reference_direction = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
+								       _lat_control_sp.course_setpoint, _lateral_control_state.wind_speed,
+								       _long_control_state.airspeed_eas * _long_control_state.eas2tas);
+			}
+
+			if (PX4_ISFINITE(_lat_control_sp.airspeed_reference_direction)) {
+				airspeed_reference_direction = PX4_ISFINITE(airspeed_reference_direction) ? airspeed_reference_direction +
+							       _lat_control_sp.airspeed_reference_direction : _lat_control_sp.airspeed_reference_direction;
+			}
+
+			if (PX4_ISFINITE(airspeed_reference_direction)) {
+				const Vector2f  airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;
+				const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
+				lateral_accel_sp = _airspeed_ref_control.controlHeading(airspeed_reference_direction, heading,
+						   _long_control_state.airspeed_eas * _long_control_state.eas2tas);
+			}
+
+			if (PX4_ISFINITE(_lat_control_sp.lateral_acceleration_setpoint)) {
+				lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + _lat_control_sp.lateral_acceleration_setpoint :
+						   _lat_control_sp.lateral_acceleration_setpoint;
+			}
+
+			if (PX4_ISFINITE(lateral_accel_sp)) {
+				lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
+				lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_limits.lateral_accel_max,
+								   _lateral_limits.lateral_accel_max);
+				roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
+			}
+
+			_att_sp.reset_integral = _lat_control_sp.reset_integral;
+			_att_sp.fw_control_yaw_wheel = PX4_ISFINITE(_lat_control_sp.heading_sp_runway_takeoff);
+			yaw_sp = _lat_control_sp.heading_sp_runway_takeoff;
+
+			fw_lateral_control_setpoint_s status = {
+				.timestamp = _lat_control_sp.timestamp,
+				.course_setpoint = _lat_control_sp.course_setpoint,
+				.airspeed_reference_direction = airspeed_reference_direction,
+				.lateral_acceleration_setpoint = lateral_accel_sp,
+				.roll_sp = roll_sp // TODO: just for logging, can be removed later
+			};
+
+			_lateral_ctrl_status_pub.publish(status);
+
+			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
+			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
+			const float yaw_body = PX4_ISFINITE(yaw_sp) ? yaw_sp : _yaw;
+			const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
+
+			if (_control_mode_sub.get().flag_control_manual_enabled) {
+				roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
+						      radians(_param_fw_r_lim.get()));
+				pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
+						       radians(_param_fw_p_lim_max.get()));
+			}
+
+			// roll slew rate
+			roll_body = _roll_slew_rate.update(roll_body, control_interval);
+
+			_att_sp.timestamp = hrt_absolute_time();
+			const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
+			q.copyTo(_att_sp.q_d);
+
+			_att_sp.thrust_body[0] = thrust_body_x;
+
+			_attitude_sp_pub.publish(_att_sp);
+		}
+
+		_z_reset_counter = _local_pos.z_reset_counter;
+	}
+
+	perf_end(_loop_perf);
+}
+
+void FwLateralLongitudinalControl::updateControlLimits()
+{
+	if (_lateral_limits.timestamp == 0) {
+		_lateral_limits.timestamp = _local_pos.timestamp;
+		_lateral_limits.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
+
+	}
+
+	if (_long_limits.timestamp == 0) {
+		setDefaultLongitudinalControlLimits();
+	}
+
+	if (_long_control_limits_sub.updated()) {
+		longitudinal_control_limits_s limits_in{};
+		_long_control_limits_sub.copy(&limits_in);
+		updateLongitudinalControlLimits(limits_in);
+	}
+
+	if (_lateral_control_limits_sub.updated()) {
+		lateral_control_limits_s limits_in{};
+		_lateral_control_limits_sub.copy(&limits_in);
+		_lateral_limits.timestamp = limits_in.timestamp;
+
+		if (PX4_ISFINITE(limits_in.lateral_accel_max)) {
+			_lateral_limits.lateral_accel_max = min(limits_in.lateral_accel_max, tanf(radians(
+					_param_fw_r_lim.get())) * CONSTANTS_ONE_G);
+
+		} else {
+			_lateral_limits.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
+		}
+
+	}
+}
+
+void
+FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+		float pitch_min_rad, float pitch_max_rad, float throttle_min,
+		float throttle_max, const float desired_max_sinkrate,
+		const float desired_max_climbrate,
+		bool disable_underspeed_detection, float hgt_rate_sp)
+{
+	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
+	if (_vehicle_status_sub.get().is_vtol
+	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		|| _vehicle_status_sub.get().in_transition_mode)) {
+		_tecs_is_running = false;
+		return;
+
+	} else {
+		_tecs_is_running = true;
+	}
+
+	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
+						throttle_max, airspeed_sp, _air_density);
+
+	/* No underspeed protection in landing mode */
+	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
+
+	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
+	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
+	const float airspeed_rate_estimate = 0.f;
+
+	_tecs.update(_long_control_state.pitch_rad - radians(_param_fw_psp_off.get()),
+		     _long_control_state.altitude_msl,
+		     alt_sp,
+		     airspeed_sp,
+		     _long_control_state.airspeed_eas,
+		     _long_control_state.eas2tas,
+		     throttle_min,
+		     throttle_max,
+		     throttle_trim_compensated,
+		     pitch_min_rad - radians(_param_fw_psp_off.get()),
+		     pitch_max_rad - radians(_param_fw_psp_off.get()),
+		     desired_max_climbrate,
+		     desired_max_sinkrate,
+		     airspeed_rate_estimate,
+		     _long_control_state.height_rate,
+		     hgt_rate_sp);
+
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
+
+	if (_tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
+	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
+		const TECS::DebugOutput &tecs_output{_tecs.getStatus()};
+
+		// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
+		if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
+		    fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
+
+		} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
+			   (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
+
+		} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
+			   (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
+
+		} else {
+			//We can't infer the flight phase , do nothing, estimation is reset at each step
+		}
+	}
+}
+
+void
+FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
+		float true_airspeed_derivative_raw, float throttle_trim)
+{
+	tecs_status_s tecs_status{};
+
+	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
+
+	tecs_status.altitude_sp = alt_sp;
+	tecs_status.altitude_reference = debug_output.altitude_reference;
+	tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
+	tecs_status.height_rate_reference = debug_output.height_rate_reference;
+	tecs_status.height_rate_direct = debug_output.height_rate_direct;
+	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
+	tecs_status.height_rate = -_local_pos.vz;
+	tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
+	tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp;
+	tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
+	tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
+	tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
+	tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
+	tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate;
+	tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
+	tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
+	tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
+	tecs_status.throttle_integ = debug_output.control.throttle_integrator;
+	tecs_status.pitch_integ = debug_output.control.pitch_integrator;
+	tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
+	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	tecs_status.throttle_trim = throttle_trim;
+	tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
+	tecs_status.fast_descend_ratio = debug_output.fast_descend;
+
+	tecs_status.timestamp = hrt_absolute_time();
+
+	_tecs_status_pub.publish(tecs_status);
+}
+
+int FwLateralLongitudinalControl::task_spawn(int argc, char *argv[])
+{
+	bool is_vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			is_vtol = true;
+		}
+	}
+
+	FwLateralLongitudinalControl *instance = new FwLateralLongitudinalControl(is_vtol);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+bool
+FwLateralLongitudinalControl::init()
+{
+	if (!_local_pos_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+int FwLateralLongitudinalControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FwLateralLongitudinalControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+void FwLateralLongitudinalControl::update_control_state() {
+	updateAltitudeAndHeightRate();
+	updateAirspeed();
+	updateAttitude();
+	updateWind();
+
+	_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
+}
+
+void FwLateralLongitudinalControl::updateWind() {
+	if (_wind_sub.updated()) {
+		wind_s wind{};
+		_wind_sub.update(&wind);
+
+		// assumes wind is valid if finite
+		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
+			      && PX4_ISFINITE(wind.windspeed_east);
+
+		_time_wind_last_received = hrt_absolute_time();
+
+		_lateral_control_state.wind_speed(0) = wind.windspeed_north;
+		_lateral_control_state.wind_speed(1) = wind.windspeed_east;
+
+	} else {
+		// 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) {
+		_lateral_control_state.wind_speed.setZero();
+	}
+}
+
+void FwLateralLongitudinalControl::updateAltitudeAndHeightRate() {
+	float ref_alt{0.f};
+	if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
+		ref_alt = _local_pos.ref_alt;
+	}
+
+	_long_control_state.altitude_msl = -_local_pos.z + ref_alt; // Altitude AMSL in meters
+	_long_control_state.height_rate = -_local_pos.vz;
+
+}
+
+void FwLateralLongitudinalControl::updateAttitude() {
+	vehicle_attitude_s att;
+
+	if (_vehicle_attitude_sub.update(&att)) {
+
+		Dcmf R{Quatf(att.q)};
+
+		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
+		// between multirotor and fixed wing flight
+		if (_vehicle_status_sub.get().is_vtol_tailsitter) {
+			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
+			R = R * R_offset;
+		}
+
+		const Eulerf euler_angles(R);
+		_long_control_state.pitch_rad = euler_angles.theta();
+		_yaw = euler_angles.psi();
+
+		// load factor due to banking
+		const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
+		_tecs.set_load_factor(load_factor_from_bank_angle);
+	}
+}
+
+void FwLateralLongitudinalControl::updateAirspeed() {
+	bool airspeed_valid = _airspeed_valid;
+	airspeed_validated_s airspeed_validated;
+
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
+
+
+		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
+		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
+
+			airspeed_valid = true;
+
+			_time_airspeed_last_valid = airspeed_validated.timestamp;
+			_long_control_state.airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
+
+			_long_control_state.eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
+
+		} else {
+			airspeed_valid = false;
+		}
+
+	} else {
+		// no airspeed updates for one second
+		if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
+			airspeed_valid = false;
+		}
+	}
+
+	// update TECS if validity changed
+	if (airspeed_valid != _airspeed_valid) {
+		_tecs.enable_airspeed(airspeed_valid);
+		_airspeed_valid = airspeed_valid;
+	}
+}
+bool FwLateralLongitudinalControl::checkLowHeightConditions() const
+{
+	// Are conditions for low-height
+	return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
+	       && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
+}
+
+void FwLateralLongitudinalControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
+{
+	// Target time constant for the TECS altitude tracker
+	float alt_tracking_tc = _param_fw_t_h_error_tc.get();
+
+	if (is_low_height) {
+		// If low-height conditions satisfied, compute target time constant for altitude tracking
+		alt_tracking_tc *= _param_fw_thrtc_sc.get();
+	}
+
+	_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
+}
+
+float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
+{
+	if (is_wind_valid) {
+		// If we have a valid wind estimate, npfg is able to handle all degenerated cases
+		return 1.f;
+	}
+
+	// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
+	// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
+	const Vector2f ground_vel(local_pos.vx, local_pos.vy);
+	const float ground_speed(ground_vel.norm());
+	const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
+									    local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
+							    0.f, 1.f));
+
+	// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
+	const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
+	const Vector2f ground_vel_norm(ground_vel.normalized());
+	const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
+							   COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
+												    COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
+							  0.f, 1.f));
+
+	return flying_forward_factor * low_ground_speed_factor;
+}
+float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
+{
+	// Scale the npfg output to zero if npfg is not certain for correct output
+	const float can_run_factor{math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f)};
+
+	hrt_abstime now{hrt_absolute_time()};
+
+	// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)
+
+	// If the npfg was not running before, reset the user warning variables.
+	if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
+		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
+	}
+
+	if (_vehicle_status_sub.get().in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) {
+		// NPFG reports a good condition or we are in transition, reset the user warning variables.
+		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
+
+	} else if (_need_report_npfg_uncertain_condition) {
+		if (_time_since_first_reduced_roll == 0U) {
+			_time_since_first_reduced_roll = now;
+		}
+
+		if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
+			_need_report_npfg_uncertain_condition = false;
+			events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
+				     "Roll command reduced due to uncertain velocity/wind estimates!");
+		}
+
+	} else {
+		// Nothing to do, already reported.
+	}
+
+	_time_since_last_npfg_call = now;
+
+	return can_run_factor * (lateral_accel_sp);
+}
+float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const {
+	return  atanf(lateral_acceleration_sp * 1.0f / CONSTANTS_ONE_G);
+}
+
+void FwLateralLongitudinalControl::setDefaultLongitudinalControlLimits() {
+		_long_limits.timestamp = hrt_absolute_time();
+		_long_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed();
+		_long_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed();
+		_long_limits.pitch_min = radians(_param_fw_p_lim_min.get());
+		_long_limits.pitch_max = radians(_param_fw_p_lim_max.get());
+		_long_limits.throttle_min = _param_fw_thr_min.get();
+		_long_limits.throttle_max = _param_fw_thr_max.get();
+		_long_limits.climb_rate_target = _param_climbrate_target.get();
+		_long_limits.sink_rate_target = _param_sinkrate_target.get();
+		_long_limits.disable_underspeed_protection = false;
+		_long_limits.enforce_low_height_condition = false;
+}
+
+void FwLateralLongitudinalControl::updateLongitudinalControlLimits(const longitudinal_control_limits_s &limits_in) {
+	_long_limits.timestamp = limits_in.timestamp;
+	if(PX4_ISFINITE(limits_in.equivalent_airspeed_min) ) {
+		_long_limits.equivalent_airspeed_min = math::constrain(limits_in.equivalent_airspeed_min, _performance_model.getMinimumCalibratedAirspeed(), _performance_model.getCalibratedTrimAirspeed());
+	} else {
+		_long_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed();
+	}
+
+	if (PX4_ISFINITE(limits_in.equivalent_airspeed_max)) {
+		_long_limits.equivalent_airspeed_max = math::constrain(limits_in.equivalent_airspeed_max, _performance_model.getCalibratedTrimAirspeed(), _performance_model.getMaximumCalibratedAirspeed());
+	} else {
+		_long_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed();
+	}
+
+	if (PX4_ISFINITE(limits_in.pitch_min)) {
+		_long_limits.pitch_min = math::constrain(limits_in.pitch_min, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()));
+	} else {
+		_long_limits.pitch_min = radians(_param_fw_p_lim_min.get());
+	}
+
+	if (PX4_ISFINITE(limits_in.pitch_max)) {
+		_long_limits.pitch_max = math::constrain(limits_in.pitch_max, _long_limits.pitch_min, radians(_param_fw_p_lim_max.get()));
+	} else {
+		_long_limits.pitch_max = radians(_param_fw_p_lim_max.get());
+	}
+
+	if (PX4_ISFINITE(limits_in.throttle_min)) {
+		_long_limits.throttle_min = math::constrain(limits_in.throttle_min, _param_fw_thr_min.get(), _param_fw_thr_max.get());
+	} else {
+		_long_limits.throttle_min = _param_fw_thr_min.get();
+	}
+
+	if (PX4_ISFINITE(limits_in.throttle_max)) {
+		_long_limits.throttle_max = math::constrain(limits_in.throttle_max, _long_limits.throttle_min, _param_fw_thr_max.get());
+	} else {
+		_long_limits.throttle_max = _param_fw_thr_max.get();
+	}
+
+	if (PX4_ISFINITE(limits_in.climb_rate_target)) {
+		_long_limits.climb_rate_target = math::max(0.0f, limits_in.climb_rate_target);
+	} else {
+		_long_limits.climb_rate_target = _param_climbrate_target.get();
+	}
+
+	if (PX4_ISFINITE(limits_in.sink_rate_target)) {
+		_long_limits.sink_rate_target = math::max(0.0f, limits_in.sink_rate_target);
+	} else {
+		_long_limits.sink_rate_target = _param_sinkrate_target.get();
+	}
+}
+
+extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[])
+{
+	return FwLateralLongitudinalControl::main(argc, argv);
+}
@@ -0,0 +1,246 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file FwLateralLongitudinalControl.hpp
+ */
+
+#ifndef PX4_FWLATERALLONGITUDINALCONTROL_HPP
+#define PX4_FWLATERALLONGITUDINALCONTROL_HPP
+
+#include <float.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
+#include <lib/npfg/CourseToAirspeedRefMapper.hpp>
+#include <lib/npfg/AirspeedReferenceController.hpp>
+#include <lib/tecs/TECS.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/flight_phase_estimation.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/fw_lateral_control_setpoint.h>
+#include <uORB/topics/fw_longitudinal_control_setpoint.h>
+#include <uORB/topics/longitudinal_control_limits.h>
+#include <uORB/topics/lateral_control_limits.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <fw_performance_model/PerformanceModel.hpp>
+#include <slew_rate/SlewRate.hpp>
+
+const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .airspeed_reference_direction = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN};
+const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude_setpoint = NAN, .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN};
+
+class FwLateralLongitudinalControl final : public ModuleBase<FwLateralLongitudinalControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FwLateralLongitudinalControl(bool is_vtol);
+
+	~FwLateralLongitudinalControl() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
+	uORB::SubscriptionData<vehicle_control_mode_s> _control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::SubscriptionData<vehicle_air_data_s> _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_landed_sub{ORB_ID(vehicle_land_detected)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _fw_lateral_ctrl_sub{ORB_ID(fw_lateral_control_setpoint)};
+	uORB::Subscription _fw_longitudinal_ctrl_sub{ORB_ID(fw_longitudinal_control_setpoint)};
+	uORB::Subscription _long_control_limits_sub{ORB_ID(longitudinal_control_limits)};
+	uORB::Subscription _lateral_control_limits_sub{ORB_ID(lateral_control_limits)};
+
+	vehicle_local_position_s _local_pos{};
+	fw_longitudinal_control_setpoint_s _long_control_sp{empty_longitudinal_control_setpoint};
+	longitudinal_control_limits_s _long_limits{};
+	fw_lateral_control_setpoint_s _lat_control_sp{empty_lateral_control_setpoint};
+	lateral_control_limits_s _lateral_limits{};
+
+	uORB::Publication <vehicle_attitude_setpoint_s> _attitude_sp_pub;
+	uORB::Publication <tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
+	uORB::PublicationData <flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
+	uORB::PublicationData <fw_lateral_control_setpoint_s> _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)};
+	uORB::PublicationData <fw_longitudinal_control_setpoint_s> _longitudinal_ctrl_status_pub{
+		ORB_ID(fw_longitudinal_control_status)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
+		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
+		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
+		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
+		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
+		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
+		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
+		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
+		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
+		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
+		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
+		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
+		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
+		(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
+		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
+		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
+		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
+		(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
+
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
+		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
+		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
+		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
+		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
+		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
+		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt
+	)
+
+
+	hrt_abstime _last_time_loop_ran{};
+	uint8_t _z_reset_counter{0};
+	bool _tecs_is_running{false};
+	bool _airspeed_valid{false};
+	uint64_t _time_airspeed_last_valid{0};
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
+	// Smooths changes in the altitude tracking error time constant value
+	SlewRate<float> _tecs_alt_time_const_slew_rate;
+	struct longitudinal_control_state {
+		float pitch_rad{0.f};
+		float altitude_msl{0.f};
+		float airspeed_eas{0.f};
+		float eas2tas{1.f};
+		float height_rate{0.f};
+	} _long_control_state{};
+
+	bool _wind_valid{false};
+	hrt_abstime _time_wind_last_received{0};
+	SlewRate<float> _roll_slew_rate;
+	float _yaw{0.f};
+	struct lateral_control_state {
+		matrix::Vector2f ground_speed;
+		matrix::Vector2f wind_speed;
+	} _lateral_control_state{};
+	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
+	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
+	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed
+	vehicle_attitude_setpoint_s _att_sp{};
+
+	bool _landed{false};
+
+	perf_counter_t _loop_perf; // loop performance counter
+
+	PerformanceModel _performance_model;
+	TECS _tecs;
+	CourseToAirspeedRefMapper _course_to_airspeed;
+	AirspeedReferenceController _airspeed_ref_control;
+
+	void parameters_update();
+	void update_control_state();
+	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+					float pitch_min_rad, float pitch_max_rad, float throttle_min,
+					float throttle_max, const float desired_max_sinkrate,
+					const float desired_max_climbrate,
+					bool disable_underspeed_detection, float hgt_rate_sp);
+
+	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
+				 float throttle_trim);
+
+	void updateAirspeed();
+
+	void updateAttitude();
+
+	void updateAltitudeAndHeightRate();
+
+	float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const;
+
+	void updateWind();
+
+	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
+
+	bool checkLowHeightConditions() const;
+
+	float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const;
+
+	float getCorrectedLateralAccelSetpoint(float lateral_accel_sp);
+
+	void setDefaultLongitudinalControlLimits();
+
+	void updateLongitudinalControlLimits(const longitudinal_control_limits_s &limits_in);
+
+	void updateControlLimits();
+};
+
+
+#endif //PX4_FWLATERALLONGITUDINALCONTROL_HPP
@@ -0,0 +1,5 @@
+menuconfig MODULES_FW_LATERAL_LONGITUDINAL_CONTROL
+	bool "fw_lateral_longitudinal_control"
+	default n
+	---help---
+		Enable support for fw_lat_lon_control
@@ -56,8 +56,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/atmosphere/atmosphere.h>
-#include <lib/npfg/npfg.hpp>
-#include <lib/tecs/TECS.hpp>
+#include <lib/npfg/DirectionalGuidance.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -98,6 +97,10 @@
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/fw_lateral_control_setpoint.h>
+#include <uORB/topics/fw_longitudinal_control_setpoint.h>
+#include <uORB/topics/longitudinal_control_limits.h>
+#include <uORB/topics/lateral_control_limits.h>

 #ifdef CONFIG_FIGURE_OF_EIGHT
 #include "figure_eight/FigureEight.hpp"
@@ -115,12 +118,6 @@ using matrix::Vector2f;
 // [m] initial distance of waypoint in front of plane in heading hold mode
 static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;

-// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;
-
-// [m] distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
-
 // [rad/s] max yawrate at which plane locks yaw for heading hold mode
 static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;

@@ -169,14 +166,7 @@ static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
 // [m/s] maximum reference altitude rate threshhold
 static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;

-// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
-static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;

-// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
-static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
-
-// [s] slew rate with which we change altitude time constant
-static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;

 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -217,18 +207,19 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
 	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
-	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
 	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
-	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 	uORB::Publication<landing_gear_s> _landing_gear_pub {ORB_ID(landing_gear)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
-	uORB::PublicationData<flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
+	uORB::PublicationData<fw_lateral_control_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fw_lateral_control_setpoint)};
+	uORB::PublicationData<fw_lateral_control_setpoint_s> _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)};
+	uORB::PublicationData<lateral_control_limits_s> _lateral_ctrl_limits_pub{ORB_ID(lateral_control_limits)};
+	uORB::PublicationData<fw_longitudinal_control_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fw_longitudinal_control_setpoint)};
+	uORB::PublicationData<longitudinal_control_limits_s> _longitudinal_ctrl_limits_pub{ORB_ID(longitudinal_control_limits)};

 	manual_control_setpoint_s _manual_control_setpoint{};
 	position_setpoint_triplet_s _pos_sp_triplet{};
@@ -276,8 +267,6 @@ private:
 	double _current_longitude{0};
 	float _current_altitude{0.f};

-	float _roll{0.f};
-	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};

@@ -397,24 +386,12 @@ private:

 	hrt_abstime _time_wind_last_received{0}; // [us]

-	// TECS
-
-	// total energy control system - airspeed / altitude control
-	TECS _tecs;
-
-	bool _tecs_is_running{false};
-
-	// Smooths changes in the altitude tracking error time constant value
-	SlewRate<float> _tecs_alt_time_const_slew_rate;
-
 	// VTOL / TRANSITION
-	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
 	float _backtrans_heading{NAN};	// used to lock the initial heading for backtransition with no position control

 	// ESTIMATOR RESET COUNTERS
 	uint8_t _xy_reset_counter{0};
-	uint8_t _z_reset_counter{0};
 	uint64_t _time_last_xy_reset{0};

 	// LATERAL-DIRECTIONAL GUIDANCE
@@ -423,10 +400,7 @@ private:
 	matrix::Vector2f _closest_point_on_path;

 	// nonlinear path following guidance - lateral-directional position control
-	NPFG _npfg;
-	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
-	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
-	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed
+	DirectionalGuidance _directional_guidance;

 	PerformanceModel _performance_model;

@@ -439,7 +413,6 @@ private:

 	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
 	float _min_current_sp_distance_xy{FLT_MAX};
-	float _target_bearing{0.0f}; // [rad]

 #ifdef CONFIG_FIGURE_OF_EIGHT
 	/* Loitering */
@@ -465,27 +438,18 @@ private:

 	// Update subscriptions
 	void airspeed_poll();
-	void control_update();
+
 	void manual_control_setpoint_poll();
 	void vehicle_attitude_poll();
 	void vehicle_command_poll();
 	void vehicle_control_mode_poll();
-	void vehicle_status_poll();
+
 	void wind_poll();

-	void status_publish();
 	void landing_status_publish();
-	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
-				 float throttle_trim);
-	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
-	float getLoadFactor();

-	/**
-	 * @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output
-	 *
-	 * @return roll setpoint
-	 */
-	float getCorrectedNpfgRollSetpoint();
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+	float getLoadFactor() const;

 	/**
 	 * @brief Sets the landing abort status and publishes landing status.
@@ -503,24 +467,6 @@ private:
 	 */
 	bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);

-	/**
-	 * @brief Get a new waypoint based on heading and distance from current position
-	 *
-	 * @param heading the heading to fly to
-	 * @param distance the distance of the generated waypoint
-	 * @param waypoint_prev the waypoint at the current position
-	 * @param waypoint_next the waypoint in the heading direction
-	 */
-	void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
-					   position_setpoint_s &waypoint_next, bool flag_init);
-
-	/**
-	 * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
-	 *
-	 * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m]
-	 */
-	float get_terrain_altitude_takeoff(float takeoff_alt);
-
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -535,13 +481,6 @@ private:
 	 */
 	void updateManualTakeoffStatus();

-	/**
-	 * @brief Update desired altitude base on user pitch stick input
-	 *
-	 * @param dt Time step
-	 */
-	void update_desired_altitude(float dt);
-
 	/**
 	 * @brief Updates timing information for landed and in-air states.
 	 *
@@ -724,9 +663,6 @@ private:
 	void control_backtransition_line_follow(const Vector2f &ground_speed,
 						const position_setpoint_s &pos_sp_curr);

-	float get_tecs_pitch();
-	float get_tecs_thrust();
-
 	float get_manual_airspeed_setpoint();

 	/**
@@ -759,38 +695,8 @@ private:
 	void publishOrbitStatus(const position_setpoint_s pos_sp);

 	SlewRate<float> _airspeed_slew_rate_controller;
-	SlewRate<float> _roll_slew_rate;

-	/**
-	 * @brief A wrapper function to call the TECS implementation
-	 *
-	 * @param control_interval Time since the last position control update [s]
-	 * @param alt_sp Altitude setpoint, AMSL [m]
-	 * @param airspeed_sp Calibrated airspeed setpoint [m/s]
-	 * @param pitch_min_rad Nominal pitch angle command minimum [rad]
-	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
-	 * @param throttle_min Minimum throttle command [0,1]
-	 * @param throttle_max Maximum throttle command [0,1]
-	 * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
-	 * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
-	 * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking
-	 * @param disable_underspeed_detection True if underspeed detection should be disabled
-	 * @param hgt_rate_sp Height rate setpoint [m/s]
-	 */
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
-					float pitch_max_rad, float throttle_min, float throttle_max,
-					const float desired_max_sink_rate, const float desired_max_climb_rate, const bool is_low_height,
-					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
-
-	/**
-	 * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
-	 *
-	 * @param roll_setpoint Unconstrained roll angle setpoint [rad]
-	 * @param altitude Vehicle altitude (AMSL) [m]
-	 * @param terrain_altitude Terrain altitude (AMSL) [m]
-	 * @return Constrained roll angle setpoint [rad]
-	 */
-	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
+	float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const;

 	/**
 	 * @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
@@ -838,21 +744,6 @@ private:
 	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
 				   const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);

-	/*
-	 * Checks if the vehicle satisfies conditions for low-height flight
-	 *
-	 * @return bool True if conditions are satisfied, false otherwise
-	 */
-	bool checkLowHeightConditions();
-
-	/*
-	 * Updates TECS altitude time constant according to the is_low_height parameter.
-	 *
-	 * @param is_low_height Boolean flag defining whether we are in low-height flight
-	 * @param dt Update time step [s]
-	 */
-	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
-
 	/*
 	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
 	 * method of the same name. Takes two waypoints, steering the vehicle to track
@@ -864,9 +755,10 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint,
-			       const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
-			       const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint,
+			const matrix::Vector2f &end_waypoint,
+			const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);

 	/*
 	 * Takes one waypoint and steers the vehicle towards this.
@@ -879,8 +771,8 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
-			      const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
+			const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);

 	/*
 	 * Line (infinite) following logic. Two points on the line are used to define the
@@ -893,8 +785,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);

 	/*
 	 * Line (infinite) following logic. One point on the line and a line bearing are used to define
@@ -907,8 +800,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);

 	/*
 	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
@@ -922,9 +816,9 @@ private:
 	 * @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);
+	DirectionalGuidanceOutput 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
@@ -939,9 +833,10 @@ private:
 	 * @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);
+	DirectionalGuidanceOutput 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.
@@ -954,10 +849,12 @@ private:
 	 * @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);
+	DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
+			const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);

 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
 		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,

 		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
@@ -980,7 +877,6 @@ private:
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
 		(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
 		(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
-		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
 		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
 		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
 		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
@@ -988,30 +884,14 @@ private:
 		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
 		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,

-		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
-		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
-		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
-		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
-		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
-		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
-		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
-		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
-		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
-		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
-		(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
-		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
-		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
-		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
 		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
 		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
-		(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
+		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,

 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
-		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,

 		(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
 		(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
@@ -1025,8 +905,6 @@ private:
 		// external parameters
 		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,

-		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
-
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,

 		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
@@ -1054,6 +932,9 @@ private:
 		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on
 	)

+	void control_idle();
+
+	float rollAngleToLateralAccel(float roll_body) const;
 };

 #endif // FIXEDWINGPOSITIONCONTROL_HPP_
@@ -48,11 +48,10 @@ static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
 static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
 static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};

-FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
+FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
 	ModuleParams(nullptr),
-	_npfg(npfg),
-	_wind_vel(wind_vel),
-	_eas2tas(eas2tas)
+	_directional_guidance(npfg),
+	_wind_vel(wind_vel)
 {

 }
@@ -64,8 +63,9 @@ void FigureEight::resetPattern()
 	_pos_passed_circle_center_along_major_axis = false;
 }

-void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-				 const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	// Sanitize inputs
 	FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)};
@@ -81,7 +81,7 @@ void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const m
 	updateSegment(curr_pos_local, valid_parameters,  pattern_points);

 	// Apply control logic based on segment
-	applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
+	return applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
 }

 FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters
@@ -118,7 +118,8 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
 		const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm();

 		// Get the normalized switch distance.
-		float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+		float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+						   parameters.loiter_radius;

 		//Far away from current figure of eight. Fly towards closer circle

@@ -194,7 +195,8 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);

 	// Get the normalized switch distance.
-	float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+	float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+					   parameters.loiter_radius;

 	// Update segment if segment exit condition has been reached
 	switch (_current_segment) {
@@ -275,56 +277,60 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	}
 }

-void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			       const FigureEightPatternParameters &parameters, float target_airspeed,
-			       const FigureEightPatternPoints &pattern_points)
+DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed,
+		const FigureEightPatternPoints &pattern_points)
 {
 	Vector2f center_to_pos_local;
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);

 	switch (_current_segment) {
 	case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
-			applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
+					   ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
 			// Follow path from north-east to south-west
-			applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
-			applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
+					   ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
 			// follow path from south-east to north-west
-			applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: {
 			// Follow path from current position to south-west
-			applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_POINT_NORTHWEST: {
 			// Follow path from current position to north-west
-			applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_UNDEFINED:
 	default:
+		return DirectionalGuidanceOutput{};
 		break;
 	}
 }
@@ -356,9 +362,10 @@ float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &pa
 	return yaw_rotation;
 }

-void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			      const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			      const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise,
+		const matrix::Vector2f &normalized_circle_offset,
+		const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;

@@ -366,8 +373,6 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset;
 	Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated;

-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);

 	const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
@@ -392,16 +397,14 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius;
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
-			  _closest_point_on_path, path_curvature);
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
+			_closest_point_on_path, path_curvature);

-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
 }

-void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
-			    const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
-			    const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
+		const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	const Dcm2f rotation_matrix(calculateRotationAngle(parameters));

@@ -414,15 +417,12 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset
 	const Vector2f end_offset_rotated = rotation_matrix * end_offset;
 	const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated;

-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 	const Vector2f path_tangent = line_segment_end_position - line_segment_start_position;
 	const Vector2f unit_path_tangent = path_tangent.normalized();
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
 	_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position,
-			  0.0f);
-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(),
+			line_segment_start_position,
+			0.0f);
 }
@@ -45,7 +45,7 @@
 #include <px4_platform_common/param.h>
 #include <lib/matrix/matrix/math.hpp>

-#include "lib/npfg/npfg.hpp"
+#include "lib/npfg/DirectionalGuidance.hpp"

 class FigureEight : public ModuleParams
 {
@@ -89,7 +89,7 @@ public:
 	 * @param[in] wind_vel is the reference to the parent wind velocity [m/s].
 	 * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion.
 	 */
-	FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas);
+	FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel, float &eas2tas);

 	/**
 	 * @brief reset the figure eight pattern.
@@ -107,20 +107,8 @@ public:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			    const FigureEightPatternParameters &parameters, float target_airspeed);
-	/**
-	 * @brief Get the roll setpoint
-	 *
-	 * @return the roll setpoint in [rad].
-	 */
-	float getRollSetpoint() const {return _roll_setpoint;};
-	/**
-	 * @brief Get the indicated airspeed setpoint
-	 *
-	 * @return the indicated airspeed setpoint in [m/s].
-	 */
-	float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;};
+	DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			const FigureEightPatternParameters &parameters, float target_airspeed);
 	/**
 	 * @brief Get the target bearing of current point on figure of eight
 	 *
@@ -175,9 +163,9 @@ private:
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 * @param[in] pattern_points are the relevant points defining the figure eight pattern.
 	 */
-	void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			  const FigureEightPatternParameters &parameters, float target_airspeed,
-			  const FigureEightPatternPoints &pattern_points);
+	DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					       const FigureEightPatternParameters &parameters, float target_airspeed,
+					       const FigureEightPatternPoints &pattern_points);
 	/**
 	 * @brief Update active segment.
 	 *
@@ -214,9 +202,10 @@ private:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			 const matrix::Vector2f &curr_pos_local,
-			 const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise,
+					      const matrix::Vector2f &normalized_circle_offset,
+					      const matrix::Vector2f &curr_pos_local,
+					      const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed);
 	/**
 	 * @brief Apply path lateral control
 	 *
@@ -227,16 +216,17 @@ private:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset,
-		       const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-		       const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset,
+					    const matrix::Vector2f &normalized_line_end_offset,
+					    const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					    const FigureEightPatternParameters &parameters, float target_airspeed);

 private:
 	/**
 	 * @brief npfg lateral control object.
 	 *
 	 */
-	NPFG &_npfg;
+	DirectionalGuidance &_directional_guidance;

 	/**
 	 * @brief Wind velocity in [m/s].
@@ -244,24 +234,9 @@ private:
 	 */
 	const matrix::Vector2f &_wind_vel;
 	/**
-	 * @brief Conversion factor from indicated to true airspeed.
-	 *
-	 */
-	const float &_eas2tas;
-	/**
-	 * @brief Roll setpoint in [rad].
-	 *
-	 */
-	float _roll_setpoint;
-	/**
-	 * @brief Indicated airspeed setpoint in [m/s].
-	 *
-	 */
-	float _indicated_airspeed_setpoint;
-	/**
-	 * @brief active figure eight position setpoint.
-	 *
-	 */
+	* @brief active figure eight position setpoint.
+	*
+	*/
 	FigureEightPatternParameters _active_parameters;

 	/**
@@ -105,23 +105,23 @@ bool RunwayTakeoff::controlYaw()
 	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }

-float RunwayTakeoff::getPitch(float external_pitch_setpoint)
+float RunwayTakeoff::getPitch()
 {
 	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
 		return math::radians(param_rwto_psp_.get());
 	}

-	return external_pitch_setpoint;
+	return NAN;
 }

-float RunwayTakeoff::getRoll(float external_roll_setpoint)
+float RunwayTakeoff::getRoll()
 {
 	// until we have enough ground clearance, set roll to 0
 	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}

-	return external_roll_setpoint;
+	return NAN;
 }

 float RunwayTakeoff::getYaw(float external_yaw_setpoint)
@@ -134,7 +134,7 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint)
 	}
 }

-float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
+float RunwayTakeoff::getThrottle(const float idle_throttle) const
 {
 	float throttle = idle_throttle;

@@ -147,19 +147,13 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external
 		break;

 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+	case RunwayTakeoffState::CLIMBOUT:
 		throttle = param_rwto_max_thr_.get();

 		break;

-	case RunwayTakeoffState::CLIMBOUT:
-		// ramp in throttle setpoint over takeoff rotation transition time
-		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
-				param_rwto_rot_time_.get());
-
-		break;
-
 	case RunwayTakeoffState::FLY:
-		throttle = external_throttle_setpoint;
+		throttle = NAN;
 	}

 	return throttle;
@@ -116,13 +116,13 @@ public:
 	 * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
 	 * @return Pitch angle setpoint (limited while plane is on runway) [rad]
 	 */
-	float getPitch(float external_pitch_setpoint);
+	float getPitch();

 	/**
 	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad]
 	 * @return Roll angle setpoint [rad]
 	 */
-	float getRoll(float external_roll_setpoint);
+	float getRoll();

 	/**
 	 * @brief Returns the appropriate yaw angle setpoint.
@@ -145,7 +145,7 @@ public:
 	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
 	 * @return Throttle setpoint, normalized [0,1]
 	 */
-	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
+	float getThrottle(const float idle_throttle) const;

 	/**
 	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]
@@ -148,6 +148,12 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_status");
 	add_optional_topic("vtol_vehicle_status", 200);
 	add_topic("wind", 1000);
+	add_topic("fw_lateral_control_setpoint");
+	add_topic("fw_lateral_control_status");
+	add_topic("fw_longitudinal_control_setpoint");
+	add_topic("fw_longitudinal_control_status");
+	add_topic("longitudinal_control_limits");
+	add_topic("lateral_control_limits");

 	// multi topics
 	add_optional_topic_multi("actuator_outputs", 100, 3);
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Balduin	6cf119535d	publish platform position on uOrb topic. reused SensorGps.msg with new topic platform_gps_position. Currently only latitude_deg, longigude_deg and altitude_ellipsoid are set. check out with listener platform_gps_position.	2025-03-05 08:57:37 +01:00
Balduin	ca051a46db	Moving platform: random rocking motion More realistic than plain constant movement. Implemented with white noise, low pass filter & feedback term ensuring we stay close enough to upright. Tuning knobs maybe not very intuitive at the moment. There are: - update rates alpha_v, alpha_w (bigger = faster movement) - amplitudes ampl_v, ampl_w (bigger = larger movement) - feedback gains pos_gains, attitude_gain (bigger = smaller movement) maybe we could remove the redundance between the latter two? Also: - overload setPlatformVelocity for different inputs - don't set aircraft pose at all (make sure with environment variable that it is on platform) - platform height = 2m defined in sdf file - rename pose -> navsat to be more precise - gz submodule update. heavier platform & initial pose.	2025-03-03 14:52:33 +01:00
Balduin	eef6da16d6	move platform to own world in Tools/simulation/gz submodule. for now I am pointing it to the PR branch moving_platform_world which contains the moving_platform world. The world contains the platform so we remove the platform definition and the code that added it dynamically. Also the env var GZ_MOVING_PLATFORM is now gone -- world name can be set with existing env var PX4_GZ_WORLD.	2025-03-03 10:37:33 +01:00
Balduin	adabdf42d0	moving platform: enable movement We now have setPlatformVelocity, which commands a velocity command to the platform. The platform handles it with the gz::sim::systems::VelocityControl plugin. We could also set the velocity once and forget about it (has no effect if doing it directly after creating it though, need to wait a bit, 2 seconds worked). But by setting it constantly we can easily change to more realistic boat-like movement.	2025-02-28 16:56:54 +01:00
Balduin	4ee8f58646	set pose of platform only updates slowly, fails often, aircraft stays in place if pose changes discontinuously. need to set velocity.	2025-02-28 10:21:01 +01:00
Balduin	a4894bde07	move platform sdf into string rather than untracked sdf file in gz submodule.	2025-02-28 09:12:15 +01:00
Balduin	10b2321e87	toggle platform with environment var	2025-02-27 17:20:29 +01:00
Balduin	4b4876128d	moving platform in gazebo: first steps the good: - we have a platform in gazebo, created dynamically from GZBridge. - it has a NavSat sensor, with a working callback on the PX4 side, GZBridge::platformNavsatCallback. From there we can publish the info as uOrb and do what we want with it. the bad: - the platform doesn't move yet the ugly: - the platforms sdf file is currently untracked in the Tools/simulation/gz submodule. Without this the commit here is useless. Need to find a sensible spot for it.	2025-02-27 15:35:48 +01:00
mahima-yoga	c19544fad3	dds-topics: add vtol_vehicle_status message, remove vehicle_air_data.	2025-02-25 14:37:19 +01:00
Silvan Fuhrer	d333661fdc	Merge branch 'main' into pr-fw_ctrl_api	2025-02-24 16:39:56 +01:00
Silvan	bc433aa3fd	boards: disable CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL for spacecraft Signed-off-by: Silvan <silvan@auterion.com>	2025-02-24 16:18:26 +01:00
Silvan Fuhrer	bc742b05f0	Merge branch 'main' into pr-fw_ctrl_api	2025-02-19 15:54:26 +01:00
RomanBapst	55be9648e8	improved handling of control limits Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-17 14:35:32 +03:00
mahimayoga	6003696b40	dds: add Wind, AirspeedValidated and VehicleAirData messages.	2025-02-17 14:35:32 +03:00
RomanBapst	954cef1f50	range finder: remove potential deadlock Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-07 16:40:04 +03:00
RomanBapst	d49ce204f7	improve unit tests for range fog detection Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-07 16:39:33 +03:00
RomanBapst	677c9c7029	Merge branch 'pr-fw_ctrl_api' of github.com:PX4/PX4-Autopilot into pr-fw_ctrl_api	2025-02-06 17:10:38 +03:00
RomanBapst	08e909ac72	publish default control limits once Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-06 17:10:08 +03:00
mahimayoga	f2a613207d	dds: add fw control api's to DDS topics yaml	2025-02-06 15:00:22 +01:00
RomanBapst	2d67e8cf81	removed some dead code Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-06 11:18:32 +03:00
RomanBapst	63b972e5f4	logger: log control limits Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-06 11:17:47 +03:00
RomanBapst	d72d899b98	fw_lat_lon_control: use control flags to determine when to run Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-02-04 14:17:43 +03:00
RomanBapst	37fe25570d	Merge branch 'main' of github.com:PX4/PX4-Autopilot into pr-fw_ctrl_api	2025-02-04 11:10:04 +03:00
RomanBapst	67ac275a19	fixed typo Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-23 16:40:00 +03:00
RomanBapst	e02c64c98f	addressed review comments Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-23 16:37:50 +03:00
RomanBapst	e4c620fcdc	enable fw lateral longitudinal board for every required board Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-14 11:40:32 +03:00
RomanBapst	df9bee4d9b	added documentation to control topics Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-09 10:33:18 +03:00
RomanBapst	daa6b1aa57	refactored fixed wing position control module to use new API Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-08 15:40:34 +03:00
RomanBapst	6b9ac65408	added new module for fixed wing lateral and longitudinal control Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-08 15:40:34 +03:00
RomanBapst	a9337c5565	refactored npfg into smaller classes Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-08 15:40:34 +03:00
RomanBapst	d8c9ffeea4	added messages for lateral and longitudinal control setpoints and corresponding limits Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-08 15:40:34 +03:00