Add path api

Fix
Offboard mode working
2026-05-24 02:07:35 +08:00 · 2026-03-16 07:16:59 -07:00 · 2026-03-14 17:54:48 -07:00 · 2026-03-14 17:05:45 -07:00 · 2026-03-14 17:04:10 -07:00 · 2026-01-11 07:41:29 -08:00
80 changed files with 2358 additions and 393 deletions
@@ -267,7 +267,7 @@ endif()

 set(package-contact "px4users@googlegroups.com")

-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)
@@ -126,15 +126,6 @@ then
 	set AUTOCNF yes
 fi

-# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value}
-env | while IFS='=' read -r line; do
-  value=${line#*=}
-  name=${line%%=*}
-  case $name in
-    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;;
-  esac
-done
-
 # multi-instance setup
 # shellcheck disable=SC2154
 param set MAV_SYS_ID $((px4_instance+1))
@@ -238,6 +229,15 @@ then
 	exit 1
 fi

+# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value}
+env | while IFS='=' read -r line; do
+  value=${line#*=}
+  name=${line%%=*}
+  case $name in
+    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;;
+  esac
+done
+
 dataman start

 # only start the simulator if not in replay mode, as both control the lockstep time
@@ -77,9 +77,6 @@ param set-default NAV_ACC_RAD 2
 param set-default RTL_DESCEND_ALT 5
 param set-default RTL_RETURN_ALT 5

-# Logging Parameters
-param set-default SDLOG_PROFILE 131
-
 # Sensors Parameters
 param set-default SENS_CM8JL65_CFG 104
 param set-default SENS_FLOW_MAXHGT 25
@@ -78,9 +78,6 @@ param set-default NAV_ACC_RAD 2
 param set-default RTL_DESCEND_ALT 5
 param set-default RTL_RETURN_ALT 5

-# Logging Parameters
-param set-default SDLOG_PROFILE 131
-
 # Sensors Parameters
 param set-default SENS_CM8JL65_CFG 202
 param set-default SENS_FLOW_MAXHGT 25
@@ -29,9 +29,6 @@ param set-default MPC_MAN_TILT_MAX 60

 param set-default THR_MDL_FAC 0.3

-# enable high-rate logging profile (helps with tuning)
-param set-default SDLOG_PROFILE 19
-
 param set-default IMU_DGYRO_CUTOFF 50
 param set-default IMU_GYRO_CUTOFF 90

@@ -16,6 +16,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_mode_manager start
+fw_guidance_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_mode_manager start
+fw_guidance_control start
 fw_lat_lon_control start vtol
 fw_autotune_attitude_control start vtol

@@ -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_MODE_MANAGER=y
+CONFIG_MODULES_FW_GUIDANCE_CONTROL=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
@@ -128,7 +128,7 @@
    - [LED Meanings](getting_started/led_meanings.md)
    - [Tune/Sound Meanings](getting_started/tunes.md)
    - [QGroundControl Flight-Readiness Status](flying/pre_flight_checks.md)
-
+  - [Asset Tracking](debug/asset_tracking.md)
 - [Hardware Selection & Setup](hardware/drone_parts.md)
  - [Flight Controllers (Autopilots)](flight_controller/index.md)
    - [Flight Controller Selection](getting_started/flight_controller_selection.md)
@@ -271,6 +271,8 @@
      - [Holybro M8N & M9N GPS](gps_compass/gps_holybro_m8n_m9n.md)
      - [Sky-Drones SmartAP GPS](gps_compass/gps_smartap.md)
    - [RTK GNSS](gps_compass/rtk_gps.md)
+      - [ARK G5 RTK GPS](dronecan/ark_g5_rtk_gps.md)
+      - [ARK G5 RTK HEADING GPS](dronecan/ark_g5_rtk_heading_gps.md)
      - [ARK RTK GPS (CAN)](dronecan/ark_rtk_gps.md)
      - [ARK RTK GPS L1 L5 (CAN)](dronecan/ark_rtk_gps_l1_l2.md)
      - [ARK X20 RTK GPS (CAN)](dronecan/ark_x20_rtk_gps.md)
@@ -902,6 +904,7 @@
  - [Licenses](contribute/licenses.md)
 - [Releases](releases/index.md)
  - [main (alpha)](releases/main.md)
+  - [1.17 (alpha)](releases/1.17.md)
  - [1.16 (stable)](releases/1.16.md)
  - [1.15](releases/1.15.md)
  - [1.14](releases/1.14.md)
@@ -74,7 +74,7 @@ For example, you might have the following settings to assign the gimbal roll, pi

 ![Gimbal Actuator config](../../assets/config/actuators/qgc_actuators_gimbal.png)

-The PWM values to use for the disarmed, maximum and minimum values can be determined in the same way as other servo, using the [Actuator Test sliders](../config/actuators.md#actuator-testing) to confirm that each slider moves the appropriate axis, and changing the values so that the gimbal is in the appropriate position at the disarmed, low and high position in the slider.
+The PWM values to use for the disarmed, maximum, center and minimum values can be determined in the same way as other servo, using the [Actuator Test sliders](../config/actuators.md#actuator-testing) to confirm that each slider moves the appropriate axis, and changing the values so that the gimbal is in the appropriate position at the disarmed, low, center and high position in the slider.
 The values may also be provided in gimbal documentation.

 ## Gimbal Control in Missions
@@ -1,6 +1,6 @@
 # Neural Networks

-<Badge type="tip" text="main (planned for: PX4 v1.17)" /> <Badge type="warning" text="Experimental" />
+<Badge type="tip" text="PX4 v1.17" /> <Badge type="warning" text="Experimental" />

 ::: warning
 This is an experimental module.
@@ -10,6 +10,10 @@ CAN it is designed to be democratic and uses differential signaling.
 For this reason it is very robust even over longer cable lengths (on large vehicles), and avoids a single point of failure.
 CAN also allows status feedback from peripherals and convenient firmware upgrades over the bus.

+PX4 has the ability to track and log detailed information from CAN devices, including firmware versions, hardware versions, and serial numbers.
+This enables unique identification and lifecycle tracking of hardware connected to the flight controller.
+See [Asset Tracking](../debug/asset_tracking.md) for more information.
+
 PX4 supports two software protocols for communicating with CAN devices:

 - [DroneCAN](../dronecan/index.md): PX4 recommends this for most common setups.
@@ -0,0 +1,68 @@
+# Asset Tracking
+
+<Badge type="tip" text="main (planned for: PX4 v1.18)" />
+
+PX4 can track and log detailed information about external hardware devices connected to the flight controller.
+This enables unique identification of vehicle parts throughout their operational lifetime using device IDs, serial numbers, and version information.
+
+::: info
+Asset tracking is currently implemented for [DroneCAN](../dronecan/index.md) devices only.
+:::
+
+## Overview
+
+Asset tracking allows you to determine exactly which hardware is installed on a vehicle, providing serial number, version, and other information.
+This makes it easier to track and maintain specific vehicle parts across multiple vehicles, to quickly see what versions you're running when debugging, and log component information for regulatory audits.
+
+Asset tracking automatically collects and logs the following metadata from external devices:
+
+- **Device identification**: Vendor name, model name, device type
+- **Version information**: Firmware version, hardware version
+- **Unique identifiers**: Serial number, device ID
+- **Device capabilities**: ESC, GPS, magnetometer, barometer, etc.
+
+This information is published via the [`device_information`](../msg_docs/DeviceInformation.md) uORB topic and logged to flight logs.
+This enables fleet management, maintenance tracking, and troubleshooting.
+
+## Viewing Device Information
+
+### Real-Time Monitoring
+
+You can view device information in real-time using the [MAVLink Shell](../debug/mavlink_shell.md) or console:
+
+```sh
+listener device_information
+```
+
+Example output for a CAN GPS module:
+
+```plain
+TOPIC: device_information
+ device_information
+    timestamp: 16258961403 (0.216525 seconds ago)
+    device_id: 8944643 (Type: 0x88, UAVCAN:0 (0x7C))
+    device_type: 5
+    vendor_name: "cubepilot"
+    model_name: "here4"
+    firmware_version: "1.14.3006590"
+    hardware_version: "4.19"
+    serial_number: "1c00410018513331"
+```
+
+Device information is published in a round-robin fashion for each detected device, at a rate of approximately 1 Hz.
+
+### Multi-Capability Devices
+
+Devices with multiple sensors (e.g., a CAN GPS/magnetometer combo module like the HERE4) register separate device information entries for each capability.
+Each entry shares the same serial number and base metadata but has a different `device_id` corresponding to the specific sensor capability.
+
+## Flight Log Analysis
+
+Device information is automatically logged to flight logs.
+You can extract it using [pyulog](../log/flight_log_analysis.md#pyulog), though note that fields like vendor name, model name, and serial number are stored as `char` arrays and require additional parsing.
+
+## See Also
+
+- [CAN (DroneCAN & Cyphal)](../can/index.md) — CAN bus configuration and setup
+- [DroneCAN](../dronecan/index.md) — DroneCAN-specific documentation
+- [Flight Log Analysis](../log/flight_log_analysis.md) — Flight log analysis
@@ -0,0 +1,112 @@
+# ARK G5 RTK GPS
+
+::: info
+This GPS module is made in the USA and NDAA compliant.
+:::
+
+[ARK G5 RTK GPS](https://arkelectron.com/product/ark-g5-rtk-gps/) is a [DroneCAN](index.md) quad-band [RTK GPS](../gps_compass/rtk_gps.md).
+
+The module incorporates the [Septentrio mosaic-G5 P3 Ultra-compact high-precision GPS/GNSS receiver module](https://www.u-blox.com/en/product/zed-x20p-module), magnetometer, barometer, IMU, and buzzer module.
+
+![ARK G5 RTK GPS](../../assets/hardware/gps/ark/ark_g5_rtk_gps.png)
+
+## Where to Buy
+
+Order this module from:
+
+- [ARK Electronics](https://arkelectron.com/product/ark-g5-rtk-gps/) (US)
+
+## Hardware Specifications
+
+- [DroneCAN](index.md) RTK GNSS, Magnetometer, Barometer, IMU, and Buzzer Module
+- [Dronecan Firmware Updating](../dronecan/index.md#firmware-update)
+- Sensors
+  - [Septentrio mosaic-G5 P3 Ultra-compact high-precision GPS/GNSS receiver module](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3)
+    - All-band all constellation GNSS receiver
+    - All-in-view satellite tracking: multi-constellation, quad-band GNSS module receiver
+    - Full raw data with positioning measurements and Galileo HAS positioning service compatibility
+    - Best-in-class RTK cm-level positioning accuracy
+    - Advanced GNSS+ algorithms
+    - 20Hz update rate
+  - [ST IIS2MDC Magnetometer](https://www.st.com/en/mems-and-sensors/iis2mdc.html)
+  - [Bosch BMP390 Barometer](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp390/)
+  - [Invensense ICM-42688-P 6-Axis IMU](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/)
+- STM32F412VGH6 MCU
+- Safety Button
+- Buzzer
+- Two CAN Connectors (Pixhawk Connector Standard 4-pin JST GH)
+- G5 "UART 2" Connector
+  - 4-pin JST GH
+  - TX, RX, PPS, GND
+- G5 USB C
+- Debug Connector (Pixhawk Connector Standard 6-pin JST SH)
+- LED Indicators
+  - GPS Fix
+  - RTK Status
+  - RGB system status
+- USA Built
+- NDAA Compliant
+- Power Requirements
+  - 5V
+    - 270mA
+- Dimensions
+  - Without Antenna
+    - 48.0mm x 40.0mm x 15.4mm
+    - 13.0g
+  - With Antenna
+    - 48.0mm x 40.0mm x 51.0mm
+    - 43.5g
+- Includes
+  - CAN Cable (Pixhawk Connector Standard 4-pin)
+  - Full-Frequency Helical GPS Antenna
+
+## Hardware Setup
+
+### Wiring
+
+The ARK G5 RTK GPS is connected to the CAN bus using a [Pixhawk connector standard](https://github.com/pixhawk/Pixhawk-Standards/blob/master/DS-009%20Pixhawk%20Connector%20Standard.pdf) 4-pin JST GH cable.
+For more information, refer to the [CAN Wiring](../can/index.md#wiring) instructions.
+
+### Mounting
+
+The recommended mounting orientation is with the connectors on the board pointing towards the **back of vehicle**.
+
+The sensor can be mounted anywhere on the frame, but you will need to specify its position, relative to vehicle centre of gravity, during [PX4 Configuration](#px4-configuration).
+
+## Firmware Setup
+
+The Septentrio G5 module firmware can be updated using the Septentrio [RxTools](https://www.septentrio.com/en/products/gps-gnss-receiver-software/rxtools) application.
+
+## Flight Controller Setup
+
+### Enabling DroneCAN
+
+In order to use the ARK G5 RTK GPS, connect it to the Pixhawk CAN bus and enable the DroneCAN driver by setting parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` for dynamic node allocation (or `3` if using [DroneCAN ESCs](../dronecan/escs.md)).
+
+The steps are:
+
+- In _QGroundControl_ set the parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` or `3` and reboot (see [Finding/Updating Parameters](../advanced_config/parameters.md)).
+- Connect ARK G5 RTK GPS CAN to the Pixhawk CAN.
+
+Once enabled, the module will be detected on boot.
+
+There is also CAN built-in bus termination via [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)
+
+### PX4 Configuration
+
+You need to set necessary [DroneCAN](index.md) parameters and define offsets if the sensor is not centred within the vehicle:
+
+- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
+- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK GPS from the vehicle's centre of gravity.
+
+## LED Meanings
+
+The GPS status lights are located to the right of the connectors:
+
+- Blinking green is GPS fix
+- Blinking blue is received corrections and RTK Float
+- Solid blue is RTK Fixed
+
+## See Also
+
+- [ARK G5 RTK GPS Documentation](https://docs.arkelectron.com/gps/ark-g5-rtk-gps) (ARK Docs)
@@ -0,0 +1,150 @@
+# ARK G5 RTK HEADING GPS
+
+::: info
+This GPS module is made in the USA and NDAA compliant.
+:::
+
+[ARK G5 RTK HEADING GPS](https://arkelectron.com/product/ark-g5-rtk-gps/) is a [DroneCAN](index.md) quad-band dual antenna [RTK GPS](../gps_compass/rtk_gps.md) that additionally provides vehicle yaw information from GPS.
+
+The module incorporates the [Septentrio mosaic-G5 P3H Ultra-compact high-precision GPS/GNSS receiver module with heading capability](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H), magnetometer, barometer, IMU, and buzzer module.
+
+![ARK G5 RTK HEADING GPS](../../assets/hardware/gps/ark/ark_g5_rtk_gps.png)
+
+## Where to Buy
+
+Order this module from:
+
+- [ARK Electronics](https://arkelectron.com/product/ark-g5-rtk-heading-gps/) (US)
+
+## Hardware Specifications
+
+- [DroneCAN](index.md) RTK GNSS, Magnetometer, Barometer, IMU, and Buzzer Module
+- [Dronecan Firmware Updating](../dronecan/index.md#firmware-update)
+- Sensors
+  - [Septentrio mosaic-G5 P3H Ultra-compact high-precision GPS/GNSS receiver module with heading capability](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H)
+    - All-band all constellation GNSS receiver
+    - All-in-view satellite tracking: multi-constellation, quad-band GNSS module receiver
+    - Full raw data with positioning measurements and Galileo HAS positioning service compatibility
+    - Best-in-class RTK cm-level positioning accuracy
+    - Advanced GNSS+ algorithms
+    - 20Hz update rate
+  - [ST IIS2MDC Magnetometer](https://www.st.com/en/mems-and-sensors/iis2mdc.html)
+  - [Bosch BMP390 Barometer](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp390/)
+  - [Invensense ICM-42688-P 6-Axis IMU](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/)
+- STM32F412VGH6 MCU
+- Safety Button
+- Buzzer
+- Two CAN Connectors (Pixhawk Connector Standard 4-pin JST GH)
+- G5 "UART 2" Connector
+  - 4-pin JST GH
+  - TX, RX, PPS, GND
+- G5 USB C
+- Debug Connector (Pixhawk Connector Standard 6-pin JST SH)
+- LED Indicators
+  - GPS Fix
+  - RTK Status
+  - RGB system status
+- USA Built
+- NDAA Compliant
+- Power Requirements
+  - 5V
+    - 270mA
+- Dimensions
+  - Without Antenna
+    - 48.0mm x 40.0mm x 15.4mm
+    - 13.0g
+  - With Antenna
+    - 48.0mm x 40.0mm x 51.0mm
+    - 43.5g
+- Includes
+  - CAN Cable (Pixhawk Connector Standard 4-pin)
+  - Full-Frequency Helical GPS Antenna
+
+## Hardware Setup
+
+### Wiring
+
+The ARK G5 RTK HEADING GPS is connected to the CAN bus using a [Pixhawk connector standard](https://github.com/pixhawk/Pixhawk-Standards/blob/master/DS-009%20Pixhawk%20Connector%20Standard.pdf) 4-pin JST GH cable.
+For more information, refer to the [CAN Wiring](../can/index.md#wiring) instructions.
+
+### Mounting
+
+The recommended mounting orientation is with the connectors on the board pointing towards the **back of vehicle**.
+
+The sensor can be mounted anywhere on the frame, but you will need to specify its position, relative to vehicle centre of gravity, during [PX4 configuration](#px4-configuration).
+
+## Firmware Setup
+
+The Septentrio G5 module firmware can be updated using the Septentrio [RxTools](https://www.septentrio.com/en/products/gps-gnss-receiver-software/rxtools) application.
+
+## Flight Controller Setup
+
+### Enabling DroneCAN
+
+In order to use the ARK G5 RTK HEADING GPS, connect it to the Pixhawk CAN bus and enable the DroneCAN driver by setting parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` for dynamic node allocation (or `3` if using [DroneCAN ESCs](../dronecan/escs.md)).
+
+The steps are:
+
+- In _QGroundControl_ set the parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` or `3` and reboot (see [Finding/Updating Parameters](../advanced_config/parameters.md)).
+- Connect ARK G5 RTK HEADING GPS CAN to the Pixhawk CAN.
+
+Once enabled, the module will be detected on boot.
+
+There is also CAN built-in bus termination via [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)
+
+### PX4 Configuration
+
+You need to set necessary [DroneCAN](index.md) parameters and define offsets if the sensor is not centred within the vehicle:
+
+- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
+- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
+- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
+- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK HEADING GPS from the vehicle's centre of gravity.
+
+### Parameter references
+
+This GPS is using ARK's private driver, the prameters below only exist on the firmware we ship the GPS with. You can set these params either in QGC or using the DroneCAN GUI Tool.
+
+#### SEP_OFFS_YAW (float)
+
+Heading offset angle for dual antenna GPS setups that support heading estimation.
+Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the Rover/ANT2 antenna is in front.
+The offset angle increases clockwise.
+Set this to 90 if the ANT2 antenna is placed on the right side of the vehicle and the Moving Base/MAIN antenna is on the left side.
+
+- Default: 0
+- Min: -360
+- Max: 360
+- Unit: degree
+
+#### SEP_OFFS_PITCH (float)
+
+Vertical offsets can be compensated for by adjusting the Pitch offset.
+Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. Since pitch is defined as the right-handed rotation about the vehicle Y axis, a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch.
+
+- Default: 0
+- Min: -90
+- Max: 90
+- Unit: degree
+
+#### SEP_OUT_RATE (enum)
+
+Configures the output rate for GNSS data messages.
+
+- -1: OnChange (Default)
+- 50: 50 ms
+- 100: 100 ms
+- 200: 200 ms
+- 500: 500 ms
+
+## LED Meanings
+
+The GPS status lights are located to the right of the connectors:
+
+- Blinking green is GPS fix
+- Blinking blue is received corrections and RTK Float
+- Solid blue is RTK Fixed
+
+## See Also
+
+- [ARK G5 RTK HEADING GPS Documentation](https://docs.arkelectron.com/gps/ark-g5-rtk-gps) (ARK Docs)
@@ -27,6 +27,8 @@ Connecting peripherals over DroneCAN has many benefits:
 - Wiring is less complicated as you can have a single bus for connecting all your ESCs and other DroneCAN peripherals.
 - Setup is easier as you configure ESC numbering by manually spinning each motor.
 - It allows users to configure and update the firmware of all CAN-connected devices centrally through PX4.
+- PX4 automatically tracks device information (vendor, model, versions, serial numbers) for maintenance and fleet management.
+  See [Asset Tracking](../debug/asset_tracking.md).

 ## Supported Hardware

@@ -1,6 +1,6 @@
 # Gain compression

-<Badge type="tip" text="main (planned for: PX4 v1.17)" />
+<Badge type="tip" text="PX4 v1.17" />

 Automatic gain compression reduces the gains of the angular-rate PID whenever oscillations are detected.
 It monitors the angular-rate controller output through a band-pass filter to identify these oscillations.
@@ -1,6 +1,6 @@
 # MicoAir743-Lite

-<Badge type="tip" text="main (planned for: PX4 v1.17)" />
+<Badge type="tip" text="PX4 v1.17" />

 :::warning
 PX4 does not manufacture this (or any) autopilot.
@@ -1,6 +1,6 @@
 # RadiolinkPIX6 Flight Controller

-<Badge type="tip" text="main (planned for: PX4 v1.17)" />
+<Badge type="tip" text="PX4 v1.17" />

 :::warning
 PX4 does not manufacture this (or any) autopilot.
@@ -1,6 +1,6 @@
-# AP-H743-R1
+# AP-H743-R1 Flight Controller

-<Badge type="tip" text="main (planned for: PX4 v1.17)" />
+<Badge type="tip" text="PX4 v1.17" />

 :::warning
 PX4 does not manufacture this (or any) autopilot.
@@ -50,6 +50,7 @@ These flight controllers are [manufacturer supported](../flight_controller/autop
 Order from [X-MAV](https://www.x-mav.cn/).

 ## Radio Control
+
 A Radio Control (RC) system is required if you want to manually control your vehicle (PX4 does not require a radio system for autonomous flight modes).

 You will need to select a compatible transmitter/receiver and then bind them so that they communicate (read the instructions that come with your specific transmitter/receiver).
@@ -59,14 +60,14 @@ CRSF receiver must be wired to a spare port (UART) on the Flight Controller. The

 ## Serial Port Mapping

-| UART   | Device     | Port          |
-| ------ | ---------- | ------------- |
-| USART1 | /dev/ttyS0 | GPS           |
-| USART2 | /dev/ttyS1 | GPS2          |
-| USART3 | /dev/ttyS2 | TELEM1        |
-| UART4  | /dev/ttyS3 | TELEM2        |
-| UART7  | /dev/ttyS4 | TELEM3        |
-| UART8  | /dev/ttyS5 | SERIAL4       |
+| UART   | Device     | Port    |
+| ------ | ---------- | ------- |
+| USART1 | /dev/ttyS0 | GPS     |
+| USART2 | /dev/ttyS1 | GPS2    |
+| USART3 | /dev/ttyS2 | TELEM1  |
+| UART4  | /dev/ttyS3 | TELEM2  |
+| UART7  | /dev/ttyS4 | TELEM3  |
+| UART8  | /dev/ttyS5 | SERIAL4 |

 ## PWM Output

@@ -133,13 +134,14 @@ The complete set of supported configurations can be found in the [Airframe Refer
 ## Debug Port

 ### SWD
+
 The [SWD interface](../debug/swd_debug.md) operate on the **FMU-DEBUG** port (`FMU-DEBUG`).

 The debug port (`FMU-DEBUG`) uses a [JST SM04B-GHS-TB](https://www.digikey.com/en/products/detail/jst-sales-america-inc/SM04B-GHS-TB/807788) connector and has the following pinout:

-| Pin     | Signal         | Volt  |
-| ------- | -------------- | ----- |
-| 1 (red) | 5V+            | +5V   |
-| 2 (blk) | FMU_SWDIO      | +3.3V |
-| 3 (blk) | FMU_SWCLK      | +3.3V |
-| 4 (blk) | GND            | GND   |
+| Pin     | Signal    | Volt  |
+| ------- | --------- | ----- |
+| 1 (red) | 5V+       | +5V   |
+| 2 (blk) | FMU_SWDIO | +3.3V |
+| 3 (blk) | FMU_SWCLK | +3.3V |
+| 4 (blk) | GND       | GND   |
@@ -2,11 +2,14 @@

 <img src="../../assets/site/position_fixed.svg" title="Position fix required (e.g. GPS)" width="30px" />

-The _Hold_ flight mode causes the vehicle to loiter (circle) around its current GPS position and maintain its current altitude.
+The _Hold_ flight mode causes the vehicle to loiter around its current GPS position and maintain its current altitude.
+
+The mode supports a [number of distinct loiter modes](#loiter-modes), which are triggered using different QGC controls or MAVLink commands.
+These allow loitering with circular and figure 8 flight paths.

 :::tip
 _Hold mode_ can be used to pause a mission or to help you regain control of a vehicle in an emergency.
-It is usually activated with a pre-programmed switch.
+It is usually activated with a pre-programmed RC switch.
 :::

 ::: info
@@ -24,24 +27,80 @@ It is usually activated with a pre-programmed switch.

 :::

-## Technical Summary
+## Loiter modes

-The aircraft circles around the GPS hold position at the current altitude.
-The vehicle will first ascend to [NAV_MIN_LTR_ALT](#NAV_MIN_LTR_ALT) if the mode is engaged below this altitude.
+### Default Loiter

-RC stick movement is ignored.
+The aircraft circles around the position at which the mode was triggered and maintain its current altitude.
+The loiter radius is set by the parameter [NAV_LOITER_RAD](#NAV_LOITER_RAD).
+Note that if the vehicle altitude is below [NAV_MIN_LTR_ALT](#NAV_MIN_LTR_ALT), it will ascend to that minimum altitude before circling.

-### Parameters
+The default loiter mode is entered when you switch to Hold mode without explicitly specifying any loiter behaviour.
+For example, if you switch to Hold mode using an RC switch, select **Hold** on the QGC flight mode selector, or activate the mode using the MAVLink [MAV_CMD_DO_SET_MODE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MODE) command.
+
+### Orbit Loiter Mode
+
+<Badge type="tip" text="PX4 v1.12" />
+
+The aircraft travels towards a _specified_ orbit center position, then circles it with a given direction and radius.
+
+This behaviour can be accessed in QGroundControl by clicking on the map in Fly view, selecting **Orbit at Location**, and configuring the radius.
+
+The behavior can be triggered using the MAVLink [MAV_CMD_DO_ORBIT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) command.
+Note that PX4 respects the specified centre point (`param5`, `param6`, `param7`), and the radius and direction (`param1`).
+PX4 ignores `param3` (Yaw behaviour) and `param4` (Orbits).
+The value of `param2` (velocity) is also ignored, but the speed can be controlled using the [MAV_CMD_DO_CHANGE_SPEED](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CHANGE_SPEED) command (constrained between `FW_AIRSPD_MAX` and `FW_AIRSPD_MIN`).
+PX4 outputs orbit status using the [ORBIT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#ORBIT_EXECUTION_STATUS) message.
+
+### Figure 8 Loiter Mode
+
+<Badge type="tip" text="PX4 v1.15" /> <Badge type="warning" text="Experimental" />
+
+The aircraft flys towards the closest point on a specified figure 8 path and then follows it.
+The path is defined by the figure 8 centre position, orientation, and radius of two circles.
+
+The feature is experimental, and is not present in PX4 firmware by default (on most flight controller boards).
+It can be included by setting the `CONFIG_FIGURE_OF_EIGHT` key in the [PX4 board configuration](../hardware/porting_guide_config.md#px4-board-configuration-kconfig) for your board and rebuilding.
+For example, this is enabled on the [default.px4board](https://github.com/PX4/PX4-Autopilot/blob/main/boards/auterion/fmu-v6s/default.px4board#L46) file for the `auterion/fmu-v6s` board.
+
+The behavior can be triggered using the MAVLink [MAV_CMD_DO_FIGURE_EIGHT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_FIGURE_EIGHT) command (PX4 respects all the parameters).
+PX4 outputs the figure 8 status using the [FIGURE_EIGHT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#FIGURE_EIGHT_EXECUTION_STATUS) message.
+
+::: info
+Figure 8 loitering is not currently supported by QGC: [QGC#12778: Need Support Figure of eight (8 figure) loitering by QGC](https://github.com/mavlink/qgroundcontrol/issues/12778).
+:::
+
+Figure 8 loitering is also available in the simulator.
+You can test it in [Gazebo](../sim_gazebo_gz/index.md) using a fixed wing frame:
+
+```sh
+make px4_sitl gz_rc_cessna
+```
+
+## Parameters

 Hold mode behaviour can be configured using the parameters below.

 | Parameter                                                                                                | Description                                                                                                   |
 | -------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------- |
-| [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD)                               | The radius of the loiter circle.                                                                              |
+| <a id="NAV_LOITER_RAD"></a>[NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD)    | The radius of the loiter circle.                                                                              |
 | <a id="NAV_MIN_LTR_ALT"></a>[NAV_MIN_LTR_ALT](../advanced_config/parameter_reference.md#NAV_MIN_LTR_ALT) | Minimum height for loiter mode (vehicle will ascend to this altitude if mode is engaged at a lower altitude). |

+## MAVLink Commands
+
+The following commands are relevant to this mode:
+
+- [MAV_CMD_DO_ORBIT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) - Switch to Hold mode and start the specified [Orbit loiter](#orbit-loiter-mode).
+  Params 2 (velocity), 3 (yaw), 4 (orbits) are ignored.
+  [ORBIT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#ORBIT_EXECUTION_STATUS) is emitted.
+- [MAV_CMD_DO_FIGURE_EIGHT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_FIGURE_EIGHT) - Switch to Hold mode and start the specified [Figure 8 loiter](#figure-8-loiter-mode).
+  All params are respected.
+  [FIGURE_EIGHT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#FIGURE_EIGHT_EXECUTION_STATUS) is emitted.
+
+Note, other commands may be supported.
+
 ## See Also

-[Hold Mode (MC)](../flight_modes_mc/hold.md)
+- [Hold Mode (MC)](../flight_modes_mc/hold.md)

 <!-- this maps to AUTO_LOITER in flight mode state machine -->
@@ -49,8 +49,8 @@ If the local position is invalid or becomes invalid while executing the takeoff,

 ::: info

- Takeoff towards a target position was added in <Badge type="tip" text="main (planned for: PX4 v1.17)" />.
- Holding wings level and ascending to clearance attitude when local position is invalid during takeoff was added in <Badge type="tip" text="main (planned for: PX4 v1.17)" />.
+- Takeoff towards a target position was added in <Badge type="tip" text="PX4 v1.17" />.
+- Holding wings level and ascending to clearance attitude when local position is invalid during takeoff was added in <Badge type="tip" text="PX4 v1.17" />.
 - QGroundControl does not support `MAV_CMD_NAV_TAKEOFF` (at time of writing).

 :::
@@ -20,52 +20,59 @@ The RTK compatible devices below that are expected to work with PX4 (it omits di
 The table indicates devices that also output yaw, and that can provide yaw when two on-vehicle units are used.
 It also highlights devices that connect via the CAN bus, and those which support PPK (Post-Processing Kinematic).

-| Device                                                                                                    |                             GPS                             | Compass  | [DroneCAN](../dronecan/index.md) | [GPS Yaw](#configuring-gps-as-yaw-heading-source) | PPK |
-| :-------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------: | :------: | :------------------------------: | :-----------------------------------------------: | :-: |
-| [ARK RTK GPS](../dronecan/ark_rtk_gps.md)                                                                 |                             F9P                             |  BMM150  |                ✓                 |                [Dual F9P][DualF9P]                |
-| [ARK RTK GPS L1 L5](../dronecan/ark_rtk_gps_l1_l2.md)                                                     |                             F9P                             |  BMM150  |                ✓                 |                              |
-| [ARK MOSAIC-X5 RTK GPS](../dronecan/ark_mosaic__rtk_gps.md)                                               |                          Mosaic-X5                          | IIS2MDC  |                ✓                 |      [Septentrio Dual Antenna][SeptDualAnt]       |
-| [ARK X20 RTK GPS](../dronecan/ark_x20_rtk_gps.md)                                                         |                             X20P                            |  BMP390  |                ✓                 |                                |
-| [CUAV C-RTK GPS](../gps_compass/rtk_gps_cuav_c-rtk.md)                                                    |                           M8P/M8N                           |    ✓     |                                  |                                                   |
-| [CUAV C-RTK2](../gps_compass/rtk_gps_cuav_c-rtk2.md)                                                      |                             F9P                             |    ✓     |                                  |                [Dual F9P][DualF9P]                |
-| [CUAV C-RTK 9Ps GPS](../gps_compass/rtk_gps_cuav_c-rtk-9ps.md)                                            |                             F9P                             |  RM3100  |                                  |                [Dual F9P][DualF9P]                |
-| [CUAV C-RTK2 PPK/RTK GNSS](../gps_compass/rtk_gps_cuav_c-rtk.md)                                          |                             F9P                             |  RM3100  |                                  |                                                   |  ✓  |
-| [CubePilot Here+ RTK GPS](../gps_compass/rtk_gps_hex_hereplus.md)                                         |                             M8P                             | HMC5983  |                                  |                                                   |
-| [CubePilot Here3 CAN GNSS GPS (M8N)](https://www.cubepilot.org/#/here/here3)                              |                             M8P                             | ICM20948 |                ✓                 |                                                   |
-| [Drotek SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html)         |                             F9P                             |  RM3100  |                                  |                [Dual F9P][DualF9P]                |
-| [DATAGNSS NANO HRTK Receiver](../gps_compass/rtk_gps_datagnss_nano_hrtk.md)                               | [D10P](https://docs.datagnss.com/gnss/gnss_module/D10P_RTK) | IST8310  |                                  |                         ✘                         |
-| [DATAGNSS GEM1305 RTK Receiver](../gps_compass/rtk_gps_gem1305.md)                                        |                           TAU951M                           | IST8310  |                                  |                         ✘                         |
-| [Femtones MINI2 Receiver](../gps_compass/rtk_gps_fem_mini2.md)                                            |                        FB672, FB6A0                         |    ✓     |                                  |                                                   |
-| [Freefly RTK GPS](../gps_compass/rtk_gps_freefly.md)                                                      |                             F9P                             | IST8310  |                                  |                                                   |
-| [Holybro H-RTK ZED-F9P RTK Rover (DroneCAN variant)](../dronecan/holybro_h_rtk_zed_f9p_gps.md)            |                             F9P                             |  RM3100  |                ✓                 |                [Dual F9P][DualF9P]                |
-| [Holybro H-RTK ZED-F9P RTK Rover](https://holybro.com/collections/h-rtk-gps/products/h-rtk-zed-f9p-rover) |                             F9P                             |  RM3100  |                                  |                [Dual F9P][DualF9P]                |
-| [Holybro H-RTK F9P Ultralight](https://holybro.com/products/h-rtk-f9p-ultralight)                         |                             F9P                             | IST8310  |                                  |                [Dual F9P][DualF9P]                |
-| [Holybro H-RTK F9P Helical or Base](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md)                          |                             F9P                             | IST8310  |                                  |                [Dual F9P][DualF9P]                |
-| [Holybro DroneCAN H-RTK F9P Helical](https://holybro.com/products/dronecan-h-rtk-f9p-helical)             |                             F9P                             |  BMM150  |                ✓                 |                [Dual F9P][DualF9P]                |
-| [Holybro H-RTK F9P Rover Lite](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md)                               |                             F9P                             | IST8310  |                                  |                                                   |     |
-| [Holybro DroneCAN H-RTK F9P Rover](https://holybro.com/products/dronecan-h-rtk-f9p-rover)                 |                             F9P                             |  BMM150  |                                  |                [Dual F9P][DualF9P]                |
-| [Holybro H-RTK M8P GNSS](../gps_compass/rtk_gps_holybro_h-rtk-m8p.md)                                     |                             M8P                             | IST8310  |                                  |
-| [Holybro H-RTK Unicore UM982 GPS](../gps_compass/rtk_gps_holybro_unicore_um982.md)                        |                            UM982                            | IST8310  |                                  |      [Unicore Dual Antenna][UnicoreDualAnt]       |
-| [LOCOSYS Hawk R1](../gps_compass/rtk_gps_locosys_r1.md)                                                   |                         MC-1612-V2b                         |          |                                  |                                                   |
-| [LOCOSYS Hawk R2](../gps_compass/rtk_gps_locosys_r2.md)                                                   |                         MC-1612-V2b                         | IST8310  |                                  |                                                   |
-| [mRo u-blox ZED-F9 RTK L1/L2 GPS](https://store.mrobotics.io/product-p/m10020d.htm)                       |                             F9P                             |    ✓     |                                  |                [Dual F9P][DualF9P]                |
-| [Navisys L1/L2 ZED-F9P RTK - Base only](https://www.navisys.com.tw/productdetail?name=GR901&class=RTK)    |                             F9P                             |          |                                  |                                                   |
-| [RaccoonLab L1/L2 ZED-F9P][RaccoonLab L1/L2 ZED-F9P]                                                      |                             F9P                             |  RM3100  |                ✓                 |                                                   |     |
-| [RaccoonLab L1/L2 ZED-F9P with external antenna][RaccnLabL1L2ZED-F9P ext_ant]                             |                             F9P                             |  RM3100  |                ✓                 |                                                   |
-| [Septentrio AsteRx-m3 Pro](../gps_compass/septentrio_asterx-rib.md)                                       |                           AsteRx                            |    ✓     |                                  |      [Septentrio Dual Antenna][SeptDualAnt]       |  ✓  |
-| [Septentrio mosaic-go](../gps_compass/septentrio_mosaic-go.md)                                            |                    mosaic X5 / mosaic H                     |    ✓     |                                  |      [Septentrio Dual Antenna][SeptDualAnt]       |  ✓  |
-| [SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html)                |                             F9P                             |    ✓     |                                  |                [Dual F9P][DualF9P]                |
-| [SparkFun GPS-RTK2 Board - ZED-F9P](https://www.sparkfun.com/products/15136)                              |                             F9P                             |    ✓     |                                  |                [Dual F9P][DualF9P]                |
-| [Trimble MB-Two](../gps_compass/rtk_gps_trimble_mb_two.md)                                                |                             F9P                             |    ✓     |                                  |                         ✓                         |     |
+| Device                                                                                                    |         GPS          | Compass  | [DroneCAN] |         [GPS Yaw]         | PPK |
+| :-------------------------------------------------------------------------------------------------------- | :------------------: | :------: | :--------: | :-----------------------: | :-: |
+| [ARK G5 RTK GPS](../dronecan/ark_g5_rtk_gps.md)                                                           |    [mosaic-G5 P3]    | IIS2MDC  |     ✓      |                           |     |
+| [ARK G5 RTK HEADING GPS](../dronecan/ark_g5_rtk_heading_gps.md)                                           |   [mosaic-G5 P3H]    | IIS2MDC  |     ✓      | [Heading Capability][mosaic-G5 P3H]  |     |
+| [ARK RTK GPS](../dronecan/ark_rtk_gps.md)                                                                 |         F9P          |  BMM150  |     ✓      |        [Dual F9P]         |     |
+| [ARK RTK GPS L1 L5](../dronecan/ark_rtk_gps_l1_l2.md)                                                     |         F9P          |  BMM150  |     ✓      |                           |     |
+| [ARK MOSAIC-X5 RTK GPS](../dronecan/ark_mosaic__rtk_gps.md)                                               |      Mosaic-X5       | IIS2MDC  |     ✓      | [Septentrio Dual Antenna] |     |
+| [ARK X20 RTK GPS](../dronecan/ark_x20_rtk_gps.md)                                                         |         X20P         | IIS2MDC  |     ✓      |                           |     |
+| [CUAV C-RTK GPS](../gps_compass/rtk_gps_cuav_c-rtk.md)                                                    |       M8P/M8N        |    ✓     |            |                           |     |
+| [CUAV C-RTK2](../gps_compass/rtk_gps_cuav_c-rtk2.md)                                                      |         F9P          |    ✓     |            |        [Dual F9P]         |     |
+| [CUAV C-RTK 9Ps GPS](../gps_compass/rtk_gps_cuav_c-rtk-9ps.md)                                            |         F9P          |  RM3100  |            |        [Dual F9P]         |     |
+| [CUAV C-RTK2 PPK/RTK GNSS](../gps_compass/rtk_gps_cuav_c-rtk.md)                                          |         F9P          |  RM3100  |            |                           |  ✓  |
+| [CubePilot Here+ RTK GPS](../gps_compass/rtk_gps_hex_hereplus.md)                                         |         M8P          | HMC5983  |            |                           |     |
+| [CubePilot Here3 CAN GNSS GPS (M8N)](https://www.cubepilot.org/#/here/here3)                              |         M8P          | ICM20948 |     ✓      |                           |     |
+| [Drotek SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html)         |         F9P          |  RM3100  |            |        [Dual F9P]         |     |
+| [DATAGNSS NANO HRTK Receiver](../gps_compass/rtk_gps_datagnss_nano_hrtk.md)                               |        [D10P]        | IST8310  |            |             ✘             |     |
+| [DATAGNSS GEM1305 RTK Receiver](../gps_compass/rtk_gps_gem1305.md)                                        |       TAU951M        | IST8310  |            |             ✘             |     |
+| [Femtones MINI2 Receiver](../gps_compass/rtk_gps_fem_mini2.md)                                            |     FB672, FB6A0     |    ✓     |            |                           |     |
+| [Freefly RTK GPS](../gps_compass/rtk_gps_freefly.md)                                                      |         F9P          | IST8310  |            |                           |     |
+| [Holybro H-RTK ZED-F9P RTK Rover (DroneCAN variant)](../dronecan/holybro_h_rtk_zed_f9p_gps.md)            |         F9P          |  RM3100  |     ✓      |        [Dual F9P]         |     |
+| [Holybro H-RTK ZED-F9P RTK Rover](https://holybro.com/collections/h-rtk-gps/products/h-rtk-zed-f9p-rover) |         F9P          |  RM3100  |            |        [Dual F9P]         |     |
+| [Holybro H-RTK F9P Ultralight](https://holybro.com/products/h-rtk-f9p-ultralight)                         |         F9P          | IST8310  |            |        [Dual F9P]         |     |
+| [Holybro H-RTK F9P Helical or Base](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md)                          |         F9P          | IST8310  |            |        [Dual F9P]         |     |
+| [Holybro DroneCAN H-RTK F9P Helical](https://holybro.com/products/dronecan-h-rtk-f9p-helical)             |         F9P          |  BMM150  |     ✓      |        [Dual F9P]         |     |
+| [Holybro H-RTK F9P Rover Lite](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md)                               |         F9P          | IST8310  |            |                           |     |
+| [Holybro DroneCAN H-RTK F9P Rover](https://holybro.com/products/dronecan-h-rtk-f9p-rover)                 |         F9P          |  BMM150  |            |        [Dual F9P]         |     |
+| [Holybro H-RTK M8P GNSS](../gps_compass/rtk_gps_holybro_h-rtk-m8p.md)                                     |         M8P          | IST8310  |            |                           |     |
+| [Holybro H-RTK Unicore UM982 GPS](../gps_compass/rtk_gps_holybro_unicore_um982.md)                        |        UM982         | IST8310  |            |  [Unicore Dual Antenna]   |     |
+| [LOCOSYS Hawk R1](../gps_compass/rtk_gps_locosys_r1.md)                                                   |     MC-1612-V2b      |          |            |                           |     |
+| [LOCOSYS Hawk R2](../gps_compass/rtk_gps_locosys_r2.md)                                                   |     MC-1612-V2b      | IST8310  |            |                           |     |
+| [mRo u-blox ZED-F9 RTK L1/L2 GPS](https://store.mrobotics.io/product-p/m10020d.htm)                       |         F9P          |    ✓     |            |        [Dual F9P]         |     |
+| [Navisys L1/L2 ZED-F9P RTK - Base only](https://www.navisys.com.tw/productdetail?name=GR901&class=RTK)    |         F9P          |          |            |                           |     |
+| [RaccoonLab L1/L2 ZED-F9P][RaccoonLab L1/L2 ZED-F9P]                                                      |         F9P          |  RM3100  |     ✓      |                           |     |
+| [RaccoonLab L1/L2 ZED-F9P with external antenna][RaccnLabL1L2ZED-F9P ext_ant]                             |         F9P          |  RM3100  |     ✓      |                           |     |
+| [Septentrio AsteRx-m3 Pro](../gps_compass/septentrio_asterx-rib.md)                                       |        AsteRx        |    ✓     |            | [Septentrio Dual Antenna] |  ✓  |
+| [Septentrio mosaic-go](../gps_compass/septentrio_mosaic-go.md)                                            | mosaic X5 / mosaic H |    ✓     |            | [Septentrio Dual Antenna] |  ✓  |
+| [SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html)                |         F9P          |    ✓     |            |        [Dual F9P]         |     |
+| [SparkFun GPS-RTK2 Board - ZED-F9P](https://www.sparkfun.com/products/15136)                              |         F9P          |    ✓     |            |        [Dual F9P]         |     |
+| [Trimble MB-Two](../gps_compass/rtk_gps_trimble_mb_two.md)                                                |         F9P          |    ✓     |            |             ✓             |     |

 <!-- links used in above table -->

 [RaccnLabL1L2ZED-F9P ext_ant]: https://docs.raccoonlab.co/guide/gps_mag_baro/gnss_external_antenna_f9p_v320.html
 [RaccoonLab L1/L2 ZED-F9P]: https://docs.raccoonlab.co/guide/gps_mag_baro/gps_l1_l2_zed_f9p.html
-[DualF9P]: ../gps_compass/u-blox_f9p_heading.md
-[SeptDualAnt]: ../gps_compass/septentrio.md#gnss-based-heading
-[UnicoreDualAnt]: ../gps_compass/rtk_gps_holybro_unicore_um982.md#enable-gps-heading-yaw
+[Dual F9P]: ../gps_compass/u-blox_f9p_heading.md
+[Septentrio Dual Antenna]: ../gps_compass/septentrio.md#gnss-based-heading
+[Unicore Dual Antenna]: ../gps_compass/rtk_gps_holybro_unicore_um982.md#enable-gps-heading-yaw
 [DATAGNSS GEM1305 RTK]: ../gps_compass/rtk_gps_gem1305.md
+[DroneCAN]: ../dronecan/index.md
+[GPS Yaw]: #configuring-gps-as-yaw-heading-source
+[mosaic-G5 P3]: https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3
+[mosaic-G5 P3H]: https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H
+[D10P]: https://docs.datagnss.com/gnss/gnss_module/D10P_RTK

 Notes:

@@ -143,6 +150,7 @@ The RTK GPS connection is essentially plug and play:
   ![survey-in](../../assets/qgc/setup/rtk/qgc_rtk_survey-in.png)

 1. Once Survey-in completes:
+
   - The RTK GPS icon changes to white and _QGroundControl_ starts to stream position data to the vehicle:

     ![RTK streaming](../../assets/qgc/setup/rtk/qgc_rtk_streaming.png)
@@ -321,7 +321,7 @@ The configuration can be done using the [UXRCE-DDS parameters](../advanced_confi
  - [UXRCE_DDS_SYNCT](../advanced_config/parameter_reference.md#UXRCE_DDS_SYNCT): Bridge time synchronization enable.
    The uXRCE-DDS client module can synchronize the timestamp of the messages exchanged over the bridge.
    This is the default configuration. In certain situations, for example during [simulations](../ros2/user_guide.md#ros-gazebo-and-px4-time-synchronization), this feature may be disabled.
-  - <Badge type="tip" text="PX4 v1.17" /> [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX): Index-based namespace definition
+  - [UXRCE_DDS_NS_IDX](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) <Badge type="tip" text="PX4 v1.17" />: Index-based namespace definition
    Setting this parameter to any value other than `-1` creates a namespace with the prefix `uav_` and the specified value, e.g. `uav_0`, `uav_1`, etc.
    See [namespace](#customizing-the-namespace) for methods to define richer or arbitrary namespaces.

@@ -426,7 +426,7 @@ will generate topics under the namespaces:

 :::

- A simple index-based namespace can be applied by setting the parameter [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) to a value between 0 and 9999.
+- A simple index-based namespace can be applied by setting the parameter [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) <Badge type="tip" text="PX4 v1.17" /> to a value between 0 and 9999.
  This will generate a namespace such as `/uav_0`, `/uav_1`, and so on.
  This technique is ideal if vehicles must be persistently associated with namespaces because their clients are automatically started through PX4.

@@ -129,6 +129,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 ### uXRCE-DDS / ROS2

 - [PX4-Autopilot#24113](https://github.com/PX4/PX4-Autopilot/pull/24113): <Badge type="warning" text="Experimental"/> [ROS 2 Message Translation Node](../ros2/px4_ros2_msg_translation_node.md) to translate PX4 messages from one definition version to another dynamically
+- <Badge type="warning" text="Experimental"/>[PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [ROS-based waypoint missions](../ros2/px4_ros2_waypoint_missions.md).
 - dds_topics: add vtol_vehicle_status ([PX4-Autopilot#24582](https://github.com/PX4/PX4-Autopilot/pull/24582))
 - dds_topics: add home_position ([PX4-Autopilot#24583](https://github.com/PX4/PX4-Autopilot/pull/24583))

@@ -138,6 +139,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 - Parameter to always start mavlink stream via USB. ([PX4-Autopilot#22234](https://github.com/PX4/PX4-Autopilot/pull/22234))
 - Refactor: MAVLink message handling in one function, reference instead of pointer to main instance ([PX4-Autopilo#23219](https://github.com/PX4/PX4-Autopilot/pull/22234))
 - mavlink log handler rewrite for improved effeciency ([PX4-Autopilo#23219](https://github.com/PX4/PX4-Autopilot/pull/22234))
+
 ### Multi-Rotor

 - [Multirotor] add yaw torque low pass filter ([PX4-Autopilot#24173](https://github.com/PX4/PX4-Autopilot/pull/24173))
@@ -0,0 +1,134 @@
+# PX4-Autopilot v1.17.0 Release Notes
+
+<Badge type="danger" text="Alpha/Beta" />
+
+<script setup>
+import { useData } from 'vitepress'
+const { site } = useData();
+</script>
+
+<div v-if="site.title !== 'PX4 Guide (main)'">
+  <div class="custom-block danger">
+    <p class="custom-block-title">This page is on a release branch, and hence probably out of date. <a href="https://docs.px4.io/main/en/releases/main.html">See the latest version</a>.</p>
+  </div>
+</div>
+
+This contains changes to PX4 planned for PX4 v1.17 (since the last major release [PX v1.16](../releases/1.16.md)).
+
+::: warning
+PX4 v1.17 is in alpha/beta testing.
+Update these notes with features that are going to be in PX4 v1.17 release.
+New features that are not expected to go into the v1.17 release are in [PX4-Autopilot `main` Release Notes](../releases/main.md).
+:::
+
+## Read Before Upgrading
+
+TBD …
+
+Please continue reading for [upgrade instructions](#upgrade-guide).
+
+## Major Changes
+
+- TBD
+
+## Upgrade Guide
+
+## Other changes
+
+### Hardware Support
+
+- **[New Hardware]** boards: [MicoAir743-Lite FC](../flight_controller/micoair743-lite.md) <!-- CHECK is this version and add PR link (or fix up doc version tag and move this) -->
+- **[New Hardware]** boards: [RadiolinkPIX6 FC](../flight_controller/radiolink_pix6.md) <!-- CHECK is this version and add PR! -->
+- **[New Hardware]** boards: [AP-H743-R1 FC](../flight_controller/x-mav_ap-h743r1.md) <!-- CHECK is this version and add PR! -->
+
+<!--
+
+### Common
+
+- [QGroundControl Bootloader Update](../advanced_config/bootloader_update.md#qgc-bootloader-update-sys-bl-update) via the [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) parameter has been re-enabled after being broken for a number of releases. ([PX4-Autopilot#25032: build: romf: fix generation of rc.board_bootloader_upgrade](https://github.com/PX4/PX4-Autopilot/pull/25032)).
+-->
+
+### Control
+
+<!--
+
+- Added new flight mode(s): [Altitude Cruise (MC)](../flight_modes_mc/altitude_cruise.md), Altitude Cruise (FW).
+  For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).
+
+-->
+
+- <Badge type="warning" text="Experimental" /> [MC Neural Network Module](../advanced/neural_networkss.md)
+
+### Estimation
+
+- TBD
+
+<!--
+
+### Sensors
+
+- Add [sbgECom INS driver](../sensor/sbgecom.md) ([PX4-Autopilot#24137](https://github.com/PX4/PX4-Autopilot/pull/24137))
+- Quick magnetometer calibration now supports specifying an arbitrary initial heading ([PX4-Autopilot#24637](https://github.com/PX4/PX4-Autopilot/pull/24637))
+
+-->
+
+### Simulation
+
+- Overhaul rover simulation:
+  - Add synthetic differential rover model: [PX4-gazebo-models#107](https://github.com/PX4/PX4-gazebo-models/pull/107)
+  - Add synthetic mecanum rover model: [PX4-gazebo-models#113](https://github.com/PX4/PX4-gazebo-models/pull/113)
+  - Update synthetic ackermann rover model: [PX4-gazebo-models#117](https://github.com/PX4/PX4-gazebo-models/pull/117)
+
+- [Simulation-in-Hardware (SIH)](../sim_sih/index.md#compatibility) <!-- Listed in https://docs.px4.io/main/en/sim_sih/#compatibility : Check the PRs -->
+  - New simulation: MC Hexacopter X
+  - New simulation: Ackermann Rover
+
+### Debug & Logging
+
+- TBD
+
+### Ethernet
+
+- TBD
+
+### uXRCE-DDS / Zenoh / ROS2
+
+- [PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [Fixed Wing lateral/longitudinal setpoint](../ros2/px4_ros2_control_interface.md#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype) (`FwLateralLongitudinalSetpointType`) and [VTOL transitions](../ros2/px4_ros2_control_interface.md#controlling-a-vtol). ([PX4-Autopilot#24056](https://github.com/PX4/PX4-Autopilot/pull/24056)).
+- [UXRCE_DDS: Simple index based namespace (UXRCE_DDS_NS_IDX)](../middleware/uxrce_dds.md#customizing-the-namespace)
+- [Zenoh (PX4 ROS 2 rmw_zenoh)](../middleware/zenoh.md)
+
+### MAVLink
+
+- TBD
+
+<!--
+
+### RC
+
+- Parse ELRS Status and Link Statistics TX messages in the CRSF parser.
+
+### Multi-Rotor
+
+- Removed parameters `MPC_{XY/Z/YAW}_MAN_EXPO` and use default value instead, as they were not deemed necessary anymore. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).
+- Renamed `MPC_HOLD_DZ` to `MAN_DEADZONE` to have it globally available in modes that allow for a dead zone. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).
+-->
+
+### VTOL
+
+- TBD
+
+### Fixed-wing
+
+- [Fixed Wing Takeoff mode](../flight_modes_fw/takeoff.md) will now keep climbing with level wings on position loss.
+  A target takeoff waypoint can be set to control takeoff course and loiter altitude. ([PX4-Autopilot#25083](https://github.com/PX4/PX4-Autopilot/pull/25083)).
+- Automatically suppress angular rate oscillations using [Gain compression](../features_fw/gain_compression.md). ([PX4-Autopilot#25840: FW rate control: add gain compression algorithm](https://github.com/PX4/PX4-Autopilot/pull/25840))
+
+### Rover
+
+- Removed deprecated rover module ([PX4-Autopilot#25054](https://github.com/PX4/PX4-Autopilot/pull/25054)).
+- Add support for [Apps & API](../flight_modes_rover/api.md) including [Rover Setpoints](../ros2/px4_ros2_control_interface.md#rover-setpoints) ([PX4-Autopilot#25074](https://github.com/PX4/PX4-Autopilot/pull/25074), [PX4-ROS2-Interface-Lib#140](https://github.com/Auterion/px4-ros2-interface-lib/pull/140)).
+- Update [rover simulation](../frames_rover/index.md#simulation) ([PX4-Autopilot#25644](https://github.com/PX4/PX4-Autopilot/pull/25644)) (see [Simulation](#simulation) release note for details).
+
+### ROS 2
+
+- TBD
@@ -2,7 +2,8 @@

 A list of PX4 release notes, they contain a list of the changes that went into each release, explaining the included features, bug fixes, deprecations and updates in detail.

- [main](../releases/main.md) (changes since v1.16)
+- [main](../releases/main.md) (changes planned for v1.18 or later)
+- [v1.17](../releases/1.17.md) (changes planned for v1.17, since v1.16)
 - [v1.16](../releases/1.16.md)
 - [v1.15](../releases/1.15.md)
 - [v1.14](../releases/1.14.md)
@@ -16,13 +16,13 @@ const { site } = useData();
 This contains changes to PX4 `main` branch since the last major release ([PX v1.16](../releases/1.16.md)).

 ::: warning
-PX4 v1.16 is in candidate-release testing, pending release.
-Update these notes with features that are going to be in `main` but not the PX4 v1.16 release.
+PX4 v1.17 is in alpha/beta testing.
+Update these notes with features that are going to be in `main` (PX4 v1.18 or later) but not the PX4 v1.17 release.
 :::

 ## Read Before Upgrading

-TBD …
+- TBD …

 Please continue reading for [upgrade instructions](#upgrade-guide).

@@ -45,8 +45,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 ### Control

 - Added new flight mode(s): [Altitude Cruise (MC)](../flight_modes_mc/altitude_cruise.md), Altitude Cruise (FW).
-  For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise
-  ](https://github.com/PX4/PX4-Autopilot/pull/25435)).
+  For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).

 ### Estimation

@@ -59,19 +58,34 @@ Please continue reading for [upgrade instructions](#upgrade-guide).

 ### Simulation

+- TBD
+
+<!-- MOVED THIS TO v1.17
+
 - Overhaul rover simulation:
  - Add synthetic differential rover model: [PX4-gazebo-models#107](https://github.com/PX4/PX4-gazebo-models/pull/107)
  - Add synthetic mecanum rover model: [PX4-gazebo-models#113](https://github.com/PX4/PX4-gazebo-models/pull/113)
  - Update synthetic ackermann rover model: [PX4-gazebo-models#117](https://github.com/PX4/PX4-gazebo-models/pull/117)

+-->
+
+### Debug & Logging
+
+- [Asset Tracking](../debug/asset_tracking.md): Automatic tracking and logging of external device information including vendor name, firmware and hardware version, serial numbers. Currently supports DroneCAN devices. ([PX4-Autopilot#25617](https://github.com/PX4/PX4-Autopilot/pull/25617))
+
 ### Ethernet

 - TBD

-### uXRCE-DDS / ROS2
+### uXRCE-DDS / Zenoh / ROS2
+
+- TBD
+
+<!-- MOVED THIS TO v1.17

 - [PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [Fixed Wing lateral/longitudinal setpoint](../ros2/px4_ros2_control_interface.md#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype) (`FwLateralLongitudinalSetpointType`) and [VTOL transitions](../ros2/px4_ros2_control_interface.md#controlling-a-vtol). ([PX4-Autopilot#24056](https://github.com/PX4/PX4-Autopilot/pull/24056)).
 - [PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [ROS-based waypoint missions](../ros2/px4_ros2_waypoint_missions.md).
+-->

 ### MAVLink

@@ -92,16 +106,26 @@ Please continue reading for [upgrade instructions](#upgrade-guide).

 ### Fixed-wing

+- TBD
+
+<!-- MOVED THIS TO v1.17
 - [Fixed Wing Takeoff mode](../flight_modes_fw/takeoff.md) will now keep climbing with level wings on position loss.
  A target takeoff waypoint can be set to control takeoff course and loiter altitude. ([PX4-Autopilot#25083](https://github.com/PX4/PX4-Autopilot/pull/25083)).
 - Automatically suppress angular rate oscillations using [Gain compression](../features_fw/gain_compression.md). ([PX4-Autopilot#25840: FW rate control: add gain compression algorithm](https://github.com/PX4/PX4-Autopilot/pull/25840))
+-->

 ### Rover

+- TBD
+
+<!-- MOVED THIS TO v1.17
+
 - Removed deprecated rover module ([PX4-Autopilot#25054](https://github.com/PX4/PX4-Autopilot/pull/25054)).
 - Add support for [Apps & API](../flight_modes_rover/api.md) ([PX4-Autopilot#25074](https://github.com/PX4/PX4-Autopilot/pull/25074), [PX4-ROS2-Interface-Lib#140](https://github.com/Auterion/px4-ros2-interface-lib/pull/140)).
 - Update [rover simulation](../frames_rover/index.md#simulation) ([PX4-Autopilot#25644](https://github.com/PX4/PX4-Autopilot/pull/25644)) (see [Simulation](#simulation) release note for details).

+-->
+
 ### ROS 2

 - TBD
@@ -341,9 +341,9 @@ The used types also define the compatibility with different vehicle types.
 The following sections provide a list of supported setpoint types:

 - [MulticopterGotoSetpointType](#go-to-setpoint-multicoptergotosetpointtype): <Badge type="warning" text="MC only" /> Smooth position and (optionally) heading control
- [FwLateralLongitudinalSetpointType](#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype): <Badge type="warning" text="FW only" /> <Badge type="tip" text="main (planned for: PX4 v1.17)" /> Direct control of lateral and longitudinal fixed wing dynamics
+- [FwLateralLongitudinalSetpointType](#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype): <Badge type="warning" text="FW only" /> <Badge type="tip" text="PX4 v1.17" /> Direct control of lateral and longitudinal fixed wing dynamics
 - [DirectActuatorsSetpointType](#direct-actuator-control-setpoint-directactuatorssetpointtype): Direct control of motors and flight surface servo setpoints
- [Rover Setpoints](#rover-setpoints): <Badge type="tip" text="main (planned for: PX4 v1.17)" /> Direct access to rover control setpoints (Position, Speed, Attitude, Rate, Throttle and Steering).
+- [Rover Setpoints](#rover-setpoints): <Badge type="tip" text="PX4 v1.17" /> Direct access to rover control setpoints (Position, Speed, Attitude, Rate, Throttle and Steering).

 :::tip
 The other setpoint types are currently experimental, and can be found in: [px4_ros2/control/setpoint_types/experimental](https://github.com/Auterion/px4-ros2-interface-lib/tree/main/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental).
@@ -410,7 +410,7 @@ _goto_setpoint->update(

 #### Fixed-Wing Lateral and Longitudinal Setpoint (FwLateralLongitudinalSetpointType)

-<Badge type="warning" text="Fixed wing only" /> <Badge type="tip" text="main (planned for: PX4 v1.17)" />
+<Badge type="warning" text="Fixed wing only" /> <Badge type="tip" text="PX4 v1.17" />

 ::: info
 This setpoint type is supported for fixed-wing vehicles and for VTOLs in fixed-wing mode.
@@ -552,7 +552,7 @@ If you want to control an actuator that does not control the vehicle's motion, b

 #### Rover Setpoints

-<Badge type="tip" text="main (planned for: PX4 v1.17)" /> <Badge type="warning" text="Experimental" />
+<Badge type="tip" text="PX4 v1.17" /> <Badge type="warning" text="Experimental" />

 The rover modules use a hierarchical structure to propagate setpoints:

@@ -586,7 +586,7 @@ An example for a rover specific drive mode using the `RoverSpeedAttitudeSetpoint

 ### Controlling a VTOL

-<Badge type="tip" text="main (planned for: PX4 v1.17)" /> <Badge type="warning" text="Experimental" />
+<Badge type="tip" text="PX4 v1.17" /> <Badge type="warning" text="Experimental" />

 To control a VTOL in an external flight mode, ensure you're returning the correct setpoint type based on the current flight configuration:

@@ -27,8 +27,8 @@ The Desktop computer is only used to display the virtual vehicle.
 - SIH for FW (airplane) and VTOL tailsitter are supported from PX4 v1.13.
 - SIH as SITL (without hardware) from PX4 v1.14.
 - SIH for Standard VTOL from PX4 v1.16.
- SIH for MC Hexacopter X from `main` (expected to be PX4 v1.17).
- SIH for Ackermann Rover from `main`.
+- SIH for MC Hexacopter X from PX4 v1.17.
+- SIH for Ackermann Rover from PX4 v1.17.

 ### Benefits

@@ -339,6 +339,7 @@ You can find a full list of available values for `PWM_MAIN_FUNCn` [here](../adva
 Alternatively, you can use the [`PWM_AUX_FUNCn`](../advanced_config/parameter_reference.md#PWM_AUX_FUNC1) parameters.

 You may also configure the output as desired:
+
 - Disarmed PWM: ([`PWM_MAIN_DISn`](../advanced_config/parameter_reference.md#PWM_MAIN_DIS1) / [`PWM_AUX_DIS1`](../advanced_config/parameter_reference.md#PWM_AUX_DIS1))
 - Minimum PWM ([`PWM_MAIN_MINn`](../advanced_config/parameter_reference.md#PWM_MAIN_MIN1) / [`PWM_AUX_MINn`](../advanced_config/parameter_reference.md#PWM_AUX_MIN1))
 - Maximum PWM ([`PWM_MAIN_MAXn`](../advanced_config/parameter_reference.md#PWM_MAIN_MAX1) / [`PWM_AUX_MAXn`](../advanced_config/parameter_reference.md#PWM_AUX_MAX1))
@@ -108,6 +108,7 @@ set(msg_files
 	GimbalManagerSetAttitude.msg
 	GimbalManagerSetManualControl.msg
 	GimbalManagerStatus.msg
+	GlobalPathSetpoint.msg
 	GpioConfig.msg
 	GpioIn.msg
 	GpioOut.msg
@@ -0,0 +1,16 @@
+# Path setpoint in NED frame
+# Input to Guidance controller
+# Needs to be kinematically consistent and feasible for smooth flight.
+# setting a value to NaN means the state should not be controlled
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp # time since system start (microseconds)
+
+float64 lat			# Latitude, (degrees)
+float64 lon			# Longitude, (degrees)
+float32 alt			# Altitude AMSL, (meters)
+# NED local world frame
+float32[3] tangent # Unit vector tangent to path
+float height_rate
+float32 curvature
@@ -184,6 +184,7 @@
 #define DRV_IMU_DEVTYPE_UAVCAN	0x87
 #define DRV_MAG_DEVTYPE_UAVCAN	0x88
 #define DRV_DIST_DEVTYPE_UAVCAN	0x89
+#define DRV_HYGRO_DEVTYPE_UAVCAN	0x8A

 #define DRV_ADC_DEVTYPE_ADS1115        0x90

@@ -43,7 +43,9 @@ const char *const UavcanAccelBridge::NAME = "accel";
 UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher),
 	_sub_imu_data(node)
-{ }
+{
+	set_device_type(DRV_ACC_DEVTYPE_UAVCAN);
+}

 int UavcanAccelBridge::init()
 {
@@ -59,7 +61,7 @@ int UavcanAccelBridge::init()

 void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	const hrt_abstime timestamp_sample = hrt_absolute_time();

@@ -87,13 +89,10 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e

 int UavcanAccelBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);

-	device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
-
-	channel->h_driver = new PX4Accelerometer(device_id.devid);
+	channel->h_driver = new PX4Accelerometer(device_id);

 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
@@ -49,7 +49,9 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublis
 	UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher),
 	_sub_air_pressure_data(node),
 	_sub_air_temperature_data(node)
-{ }
+{
+	set_device_type(DRV_BARO_DEVTYPE_UAVCAN);
+}

 int UavcanBarometerBridge::init()
 {
@@ -91,7 +93,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 {
 	const hrt_abstime timestamp_sample = hrt_absolute_time();

-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -105,23 +107,18 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 		return;
 	}

-	DeviceId device_id{};
-	device_id.devid_s.bus = 0;
-	device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
-
-	device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	uint32_t device_id = make_uavcan_device_id(msg);

 	// Register barometer capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {
-		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid,
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id,
 				NodeInfoPublisher::DeviceCapability::BAROMETER);
 	}

 	// publish
 	sensor_baro_s sensor_baro{};
 	sensor_baro.timestamp_sample = timestamp_sample;
-	sensor_baro.device_id = device_id.devid;
+	sensor_baro.device_id = device_id;
 	sensor_baro.pressure = msg.static_pressure;

 	if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) {
@@ -49,6 +49,7 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode
 	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher),
 	_sub_air(node)
 {
+	set_device_type(DRV_DIFF_PRESS_DEVTYPE_UAVCAN);
 }

 int UavcanDifferentialPressureBridge::init()
@@ -68,8 +69,6 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 {
 	const hrt_abstime timestamp_sample = hrt_absolute_time();

-	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
-	_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
 	float diff_press_pa = msg.differential_pressure;
 	int32_t differential_press_rev = 0;
 	param_get(param_find("SENS_DPRES_REV"), &differential_press_rev);
@@ -83,7 +82,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const

 	differential_pressure_s report{};
 	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id.devid;
+	report.device_id = make_uavcan_device_id(msg);
 	report.differential_pressure_pa = diff_press_pa;
 	report.temperature = temperature_c;
 	report.timestamp = hrt_absolute_time();
@@ -41,6 +41,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_
 	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher),
 	_sub_flow(node)
 {
+	set_device_type(DRV_FLOW_DEVTYPE_UAVCAN);
 }

 int
@@ -61,13 +62,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex:
 	sensor_optical_flow_s flow{};
 	flow.timestamp_sample = hrt_absolute_time(); // TODO

-	device::Device::DeviceId device_id;
-	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
-	device_id.devid_s.bus = 0;
-	device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
-
-	flow.device_id = device_id.devid;
+	flow.device_id = make_uavcan_device_id(msg);

 	flow.pixel_flow[0] = msg.flow_integral[0];
 	flow.pixel_flow[1] = msg.flow_integral[1];
@@ -404,7 +404,8 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 				    const uint8_t spoofing_state)
 {
 	sensor_gps_s sensor_gps{};
-	sensor_gps.device_id = get_device_id();
+
+	sensor_gps.device_id = make_uavcan_device_id(msg);

 	// Register GPS capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {
@@ -42,6 +42,7 @@ UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfo
 	UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher),
 	_sub_rel_pos_heading(node)
 {
+	set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
 }

 int
@@ -70,7 +71,7 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const
 	sensor_gnss_relative.position_length = msg.relative_distance_m;
 	sensor_gnss_relative.position[2] = msg.relative_down_pos_m;

-	sensor_gnss_relative.device_id = get_device_id();
+	sensor_gnss_relative.device_id = make_uavcan_device_id(msg);

 	// Register GPS capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {
@@ -43,7 +43,9 @@ const char *const UavcanGyroBridge::NAME = "gyro";
 UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher),
 	_sub_imu_data(node)
-{ }
+{
+	set_device_type(DRV_GYR_DEVTYPE_UAVCAN);
+}

 int UavcanGyroBridge::init()
 {
@@ -59,7 +61,7 @@ int UavcanGyroBridge::init()

 void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -87,13 +89,10 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq

 int UavcanGyroBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);

-	device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
-
-	channel->h_driver = new PX4Gyroscope(device_id.devid);
+	channel->h_driver = new PX4Gyroscope(device_id);

 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
@@ -42,6 +42,7 @@ UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPubl
 	UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher),
 	_sub_hygro(node)
 {
+	set_device_type(DRV_HYGRO_DEVTYPE_UAVCAN);
 }

 int UavcanHygrometerBridge::init()
@@ -64,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure<dr

 	sensor_hygrometer_s report{};
 	report.timestamp_sample = timestamp_sample;
-	report.device_id = 0; // TODO
+	report.device_id = make_uavcan_device_id(msg);
 	report.temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius;
 	report.humidity = msg.humidity;
 	report.timestamp = hrt_absolute_time();
@@ -49,6 +49,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node, NodeInfo
 	_sub_mag(node),
 	_sub_mag2(node)
 {
+	set_device_type(DRV_MAG_DEVTYPE_UAVCAN);
 }

 int UavcanMagnetometerBridge::init()
@@ -73,7 +74,7 @@ int UavcanMagnetometerBridge::init()
 void UavcanMagnetometerBridge::mag_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -104,7 +105,7 @@ void
 UavcanMagnetometerBridge::mag2_sub_cb(const
 				      uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	if (channel == nullptr || channel->instance < 0) {
 		// Something went wrong - no channel to publish on; return
@@ -134,13 +135,10 @@ UavcanMagnetometerBridge::mag2_sub_cb(const

 int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);

-	device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
-
-	channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE);
+	channel->h_driver = new PX4Magnetometer(device_id, ROTATION_NONE);

 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
@@ -45,7 +45,9 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder";
 UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher),
 	_sub_range_data(node)
-{ }
+{
+	set_device_type(DRV_DIST_DEVTYPE_UAVCAN);
+}

 int UavcanRangefinderBridge::init()
 {
@@ -66,7 +68,7 @@ int UavcanRangefinderBridge::init()
 void UavcanRangefinderBridge::range_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::range_sensor::Measurement> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());

 	if (channel == nullptr || channel->instance < 0) {
 		// Something went wrong - no channel to publish on; return
@@ -125,13 +127,10 @@ void UavcanRangefinderBridge::range_sub_cb(const

 int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);

-	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
-
-	channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	channel->h_driver = new PX4Rangefinder(device_id, distance_sensor_s::ROTATION_DOWNWARD_FACING);

 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
@@ -316,7 +316,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report)
 	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }

-uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id)
+uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id, uint8_t iface_index)
 {
 	uavcan_bridge::Channel *channel = nullptr;

@@ -354,6 +354,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id

 		// initialize the driver, which registers the class device name and uORB publisher
 		channel->node_id = node_id;
+		channel->iface_index = iface_index;
 		int ret = init_driver(channel);

 		if (ret != PX4_OK) {
@@ -92,6 +92,7 @@ struct Channel {
 	orb_advert_t orb_advert{nullptr};
 	int instance{-1};
 	void *h_driver{nullptr};
+	uint8_t iface_index{0};
 };
 } // namespace uavcan_bridge

@@ -138,7 +139,34 @@ protected:
 	 */
 	virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; };

-	uavcan_bridge::Channel *get_channel_for_node(int node_id);
+	uavcan_bridge::Channel *get_channel_for_node(int node_id, uint8_t iface_index);
+
+	/**
+	 * Builds a unique device ID from a UAVCAN message
+	 * @param msg UAVCAN message (must have getSrcNodeID() and getIfaceIndex() methods)
+	 * @return Complete device ID with node address and interface encoded
+	 */
+	template<typename T>
+	uint32_t make_uavcan_device_id(const uavcan::ReceivedDataStructure<T> &msg) const
+	{
+		return make_uavcan_device_id(msg.getSrcNodeID().get(), msg.getIfaceIndex());
+	}
+
+	/**
+	 * Builds a unique device ID from node ID and interface index
+	 * @param node_id UAVCAN node ID
+	 * @param iface_index CAN interface index (0 = CAN1, 1 = CAN2, etc.)
+	 * @return Complete device ID with node address and interface encoded
+	 */
+	uint32_t make_uavcan_device_id(uint8_t node_id, uint8_t iface_index) const
+	{
+		device::Device::DeviceId device_id{};
+		device_id.devid_s.devtype = get_device_type();
+		device_id.devid_s.address = node_id;
+		device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN;
+		device_id.devid_s.bus = iface_index;
+		return device_id.devid;
+	}

 public:
 	virtual ~UavcanSensorBridgeBase();
@@ -545,8 +545,10 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const

 	float output = _disarmed_value[i];

-	if (_function_assignment[i] >= OutputFunction::Servo1
-	    && _function_assignment[i] <= OutputFunction::ServoMax
+	if (((_function_assignment[i] >= OutputFunction::Servo1
+	      && _function_assignment[i] <= OutputFunction::ServoMax) || _function_assignment[i] == OutputFunction::Landing_Gear_Wheel
+	     || (_function_assignment[i] >= OutputFunction::Gimbal_Roll
+		 && _function_assignment[i] <= OutputFunction::Gimbal_Yaw))
 	    && _param_handles[i].center != PARAM_INVALID
 	    && _center_value[i] >= 800
 	    && _center_value[i] <= 2200) {
@@ -438,7 +438,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
 	}

 	if (user_config.store(param, new_value)) {
-		params_unsaved.set(param, !mark_saved);
+		params_unsaved.set(param, !mark_saved && param_changed);
 		result = PX4_OK;

 	} else {
@@ -35,6 +35,7 @@

 #include "framework.h"
 #include <uORB/topics/vehicle_status.h>
+#include "../ModeUtil/mode_requirements.hpp"

 // to run: make tests TESTFILTER=failsafe_test

@@ -50,16 +51,16 @@ protected:
 	void checkStateAndMode(const hrt_abstime &time_us, const State &state,
 			       const failsafe_flags_s &status_flags) override
 	{
-		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
-			       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
+		CHECK_FAILSAFE(status_flags, manual_control_signal_lost, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
 		CHECK_FAILSAFE(status_flags, gcs_connection_lost, Action::Descend);

 		if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
 			CHECK_FAILSAFE(status_flags, mission_failure, Action::Descend);
 		}

-		CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
-			       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
+		CHECK_FAILSAFE(status_flags, wind_limit_exceeded, ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
+		CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::RemainingFlightTimeLow));
+		CHECK_FAILSAFE(status_flags, offboard_control_signal_lost, ActionOptions(Action::Hold));

 		_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
 						 && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
@@ -258,6 +259,77 @@ TEST_F(FailsafeTest, takeover_denied)
 	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
 }

+TEST_F(FailsafeTest, can_takeover_degraded_failsafe)
+{
+	FailsafeTester failsafe(nullptr);
+
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 3847124342;
+	failsafe_flags_s failsafe_flags{};
+	mode_util::getModeRequirements(state.vehicle_type, failsafe_flags); // Load mode requirements to degrade without valid position estimate
+	bool user_intended_mode_updated = false;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+
+	// Battery time low -> Hold for the delay
+	time += 10_ms;
+	failsafe_flags.battery_low_remaining_time = true;
+	updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+
+	// Delay over -> RTL
+	time += 5_s;
+	failsafe_flags.battery_low_remaining_time = true;
+	updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+
+	// Global position gets invalid -> Land
+	time += 10_ms;
+	failsafe_flags.global_position_invalid = true;
+	updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Land);
+
+	// User wants takeover -> Altitude mode + Warning
+	time += 10_ms;
+	user_intended_mode_updated = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+	updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn);
+	ASSERT_TRUE(failsafe.userTakeoverActive());
+}
+
+TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch)
+{
+	FailsafeTester failsafe(nullptr);
+
+	failsafe_flags_s failsafe_flags{};
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 3847124342;
+	bool user_intended_mode_updated = false;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+
+	// Switch to offboard but no offboard signal -> No immediate user takeover flagged but rather Hold
+	time += 10_ms;
+	user_intended_mode_updated = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
+	failsafe_flags.offboard_control_signal_lost = true;
+	updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+	ASSERT_FALSE(failsafe.userTakeoverActive());
+}
+
 TEST_F(FailsafeTest, defer)
 {
 	FailsafeTester failsafe(nullptr);
@@ -499,9 +499,9 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 		allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
 	}

-	// User takeover is activated on user intented mode update (w/o action change, so takeover is not immediately
-	// requested when entering failsafe) or rc stick movements
-	bool want_user_takeover_mode_switch = user_intended_mode_updated && _selected_action == selected_action;
+	// User takeover interrupting a failsafe is triggered by a change of the user-intended mode
+	// (only if a failsafe action is already active otherwise there can be immediate takeover when entering a failsafe) or by stick movement
+	bool want_user_takeover_mode_switch = user_intended_mode_updated && (_selected_action > Action::Warn);
 	bool want_user_takeover = want_user_takeover_mode_switch || rc_sticks_takeover_request;
 	bool takeover_allowed =
 		(allow_user_takeover == UserTakeoverAllowed::Always && (_user_takeover_active || want_user_takeover))
@@ -59,7 +59,7 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
 		_param_handles[i].scale_spoiler = param_find(buffer);
 	}

-	_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
+	_flaps_setpoint_with_slewrate.setSlewRate(_param_ca_flap_slew.get());
 	_spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate);

 	_count_handle = param_find("CA_SV_CS_COUNT");
@@ -75,6 +75,9 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		return;
 	}

+	// Update flap slewrates
+	_flaps_setpoint_with_slewrate.setSlewRate(_param_ca_flap_slew.get());
+
 	// Helper to check if a PWM center parameter is enabled, and clamp it to valid range
 	auto check_pwm_center = [](const char *prefix, int channel) -> bool {
 		char param_name[20];
@@ -38,7 +38,6 @@
 #include <px4_platform_common/module_params.h>
 #include <lib/slew_rate/SlewRate.hpp>

-static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s]
 static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s]

 class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
@@ -111,4 +110,7 @@ private:

 	SlewRate<float> _flaps_setpoint_with_slewrate;
 	SlewRate<float> _spoilers_setpoint_with_slewrate;
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_SV_FLAP_SLEW>) _param_ca_flap_slew
+	)
 };
@@ -336,6 +336,15 @@ parameters:
            instance_start: 0
            default: 0

+        CA_SV_FLAP_SLEW:
+            description:
+                short: Control Surface slew rate for normalized flaps setpoint
+            type: float
+            decimal: 1
+            min: 0.0
+            max: 5.0
+            default: 0.5
+
        CA_SV_CS${i}_SPOIL:
            description:
                short: Control Surface ${i} configuration as spoiler
@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2015-2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__fw_guidance_control
+	MAIN fw_guidance_control
+	SRCS
+		FixedWingGuidanceControl.cpp
+		FixedWingGuidanceControl.hpp
+		ControllerConfigurationHandler.cpp
+		ControllerConfigurationHandler.hpp
+	DEPENDS
+		npfg
+		SlewRate
+		tecs
+		motion_planning
+		performance_model
+		Sticks
+	)
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 CombinedControllerConfigurationHandler.cpp
+ */
+
+#include "ControllerConfigurationHandler.hpp"
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+
+void CombinedControllerConfigurationHandler::update(const hrt_abstime now)
+{
+	_longitudinal_updated = floatValueChanged(_longitudinal_configuration_current_cycle.pitch_min,
+				_longitudinal_publisher.get().pitch_min);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.pitch_max,
+				 _longitudinal_publisher.get().pitch_max);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_min,
+				 _longitudinal_publisher.get().throttle_min);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_max,
+				 _longitudinal_publisher.get().throttle_max);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.speed_weight,
+				 _longitudinal_publisher.get().speed_weight);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.climb_rate_target,
+				 _longitudinal_publisher.get().climb_rate_target);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.sink_rate_target,
+				 _longitudinal_publisher.get().sink_rate_target);
+	_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.enforce_low_height_condition,
+				 _longitudinal_publisher.get().enforce_low_height_condition);
+	_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.disable_underspeed_protection,
+				 _longitudinal_publisher.get().disable_underspeed_protection);
+
+	_lateral_updated |= floatValueChanged(_lateral_configuration_current_cycle.lateral_accel_max,
+					      _lateral_publisher.get().lateral_accel_max);
+
+	if (_longitudinal_updated || now - _time_last_longitudinal_publish > 1_s) {
+		_longitudinal_configuration_current_cycle.timestamp = now;
+		_longitudinal_publisher.update(_longitudinal_configuration_current_cycle);
+		_time_last_longitudinal_publish = _longitudinal_configuration_current_cycle.timestamp;
+	}
+
+	if (_lateral_updated || now - _time_last_lateral_publish > 1_s) {
+		_lateral_configuration_current_cycle.timestamp = now;
+		_lateral_publisher.update(_lateral_configuration_current_cycle);
+		_time_last_lateral_publish = _lateral_configuration_current_cycle.timestamp;
+	}
+
+	_longitudinal_updated = _lateral_updated = false;
+	_longitudinal_configuration_current_cycle = empty_longitudinal_control_configuration;
+	_lateral_configuration_current_cycle = empty_lateral_control_configuration;
+}
+
+void CombinedControllerConfigurationHandler::setThrottleMax(float throttle_max)
+{
+	_longitudinal_configuration_current_cycle.throttle_max = throttle_max;
+}
+
+void CombinedControllerConfigurationHandler::setThrottleMin(float throttle_min)
+{
+	_longitudinal_configuration_current_cycle.throttle_min = throttle_min;
+}
+
+void CombinedControllerConfigurationHandler::setSpeedWeight(float speed_weight)
+{
+	_longitudinal_configuration_current_cycle.speed_weight = speed_weight;
+}
+
+void CombinedControllerConfigurationHandler::setPitchMin(const float pitch_min)
+{
+	_longitudinal_configuration_current_cycle.pitch_min = pitch_min;
+}
+
+void CombinedControllerConfigurationHandler::setPitchMax(const float pitch_max)
+{
+	_longitudinal_configuration_current_cycle.pitch_max = pitch_max;
+}
+
+void CombinedControllerConfigurationHandler::setClimbRateTarget(float climbrate_target)
+{
+	_longitudinal_configuration_current_cycle.climb_rate_target = climbrate_target;
+}
+
+void CombinedControllerConfigurationHandler::setDisableUnderspeedProtection(bool disable)
+{
+	_longitudinal_configuration_current_cycle.disable_underspeed_protection = disable;
+}
+
+void CombinedControllerConfigurationHandler::setSinkRateTarget(const float sinkrate_target)
+{
+	_longitudinal_configuration_current_cycle.sink_rate_target = sinkrate_target;
+}
+
+void CombinedControllerConfigurationHandler::setLateralAccelMax(float lateral_accel_max)
+{
+	_lateral_configuration_current_cycle.lateral_accel_max = lateral_accel_max;
+}
+
+void CombinedControllerConfigurationHandler::setEnforceLowHeightCondition(bool low_height_condition)
+{
+	_longitudinal_configuration_current_cycle.enforce_low_height_condition = low_height_condition;
+}
+
+void CombinedControllerConfigurationHandler::resetLastPublishTime()
+{
+	_time_last_longitudinal_publish = _time_last_lateral_publish = 0;
+}
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 CombinedControllerConfigurationHandler.hpp
+ */
+#ifndef PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
+#define PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
+
+#include <uORB/topics/longitudinal_control_configuration.h>
+#include <uORB/topics/lateral_control_configuration.h>
+#include <uORB/Publication.hpp>
+#include <lib/mathlib/mathlib.h>
+
+static constexpr longitudinal_control_configuration_s empty_longitudinal_control_configuration = {.timestamp = 0, .pitch_min = NAN, .pitch_max = NAN, .throttle_min = NAN, .throttle_max = NAN, .climb_rate_target = NAN, .sink_rate_target = NAN, .speed_weight = NAN, .enforce_low_height_condition = false, .disable_underspeed_protection = false };
+static constexpr lateral_control_configuration_s empty_lateral_control_configuration = {.timestamp = 0, .lateral_accel_max = NAN};
+
+
+class CombinedControllerConfigurationHandler
+{
+public:
+	CombinedControllerConfigurationHandler() = default;
+	~CombinedControllerConfigurationHandler() = default;
+
+	void update(const hrt_abstime now);
+
+	void setEnforceLowHeightCondition(bool low_height_condition);
+
+	void setThrottleMax(float throttle_max);
+
+	void setThrottleMin(float throttle_min);
+
+	void setSpeedWeight(float speed_weight);
+
+	void setPitchMin(const float pitch_min);
+
+	void setPitchMax(const float pitch_max);
+
+	void setClimbRateTarget(float climbrate_target);
+
+	void setDisableUnderspeedProtection(bool disable);
+
+	void setSinkRateTarget(const float sinkrate_target);
+
+	void setLateralAccelMax(float lateral_accel_max);
+
+	void resetLastPublishTime();
+
+private:
+	bool booleanValueChanged(bool new_value, bool current_value)
+	{
+		return current_value != new_value;
+	}
+
+	bool floatValueChanged(float new_value, float current_value)
+	{
+		bool  diff;
+
+		if (PX4_ISFINITE(new_value)) {
+			diff = PX4_ISFINITE(current_value) ? (fabsf(new_value - current_value) > FLT_EPSILON) : true;
+
+		} else {
+			diff = PX4_ISFINITE(current_value);
+		}
+
+		return diff;
+	}
+
+	bool _lateral_updated{false};
+	bool _longitudinal_updated{false};
+
+	hrt_abstime _time_last_longitudinal_publish{0};
+	hrt_abstime _time_last_lateral_publish{0};
+
+	uORB::PublicationData<lateral_control_configuration_s> _lateral_publisher{ORB_ID(lateral_control_configuration)};
+	uORB::PublicationData<longitudinal_control_configuration_s> _longitudinal_publisher{ORB_ID(longitudinal_control_configuration)};
+
+	lateral_control_configuration_s _lateral_configuration_current_cycle{empty_lateral_control_configuration};
+	longitudinal_control_configuration_s _longitudinal_configuration_current_cycle {empty_longitudinal_control_configuration};
+};
+
+#endif //PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
@@ -0,0 +1,538 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 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 "FixedWingGuidanceControl.hpp"
+
+#include <px4_platform_common/events.h>
+#include <uORB/topics/longitudinal_control_configuration.h>
+
+using math::constrain;
+using math::max;
+using math::min;
+using math::radians;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector2d;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+const fixed_wing_lateral_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course = NAN, .airspeed_direction = NAN, .lateral_acceleration = NAN};
+const fixed_wing_longitudinal_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude = NAN, .height_rate = NAN, .equivalent_airspeed = NAN, .pitch_direct = NAN, .throttle_direct = NAN};
+
+FixedWingGuidanceControl::FixedWingGuidanceControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	// limit to 50 Hz
+	_local_pos_sub.set_interval_ms(20);
+
+	_launch_detection_status_pub.advertise();
+	_fixed_wing_lateral_guidance_status_pub.advertise();
+
+	parameters_update();
+}
+
+FixedWingGuidanceControl::~FixedWingGuidanceControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+FixedWingGuidanceControl::init()
+{
+	if (!_local_pos_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+void
+FixedWingGuidanceControl::parameters_update()
+{
+	updateParams();
+
+	_directional_guidance.setPeriod(_param_npfg_period.get());
+	_directional_guidance.setDamping(_param_npfg_damping.get());
+	_directional_guidance.enablePeriodLB(_param_npfg_en_period_lb.get());
+	_directional_guidance.enablePeriodUB(_param_npfg_en_period_ub.get());
+	_directional_guidance.setRollTimeConst(_param_npfg_roll_time_const.get());
+	_directional_guidance.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
+	_directional_guidance.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
+}
+
+void
+FixedWingGuidanceControl::vehicle_control_mode_poll()
+{
+	if (_control_mode_sub.updated()) {
+		_control_mode_sub.copy(&_control_mode);
+	}
+}
+
+void
+FixedWingGuidanceControl::airspeed_poll()
+{
+	airspeed_validated_s airspeed_validated;
+
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
+
+		// do not use synthetic airspeed as it's for the use here not reliable enough
+		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
+		    && airspeed_validated.airspeed_source != airspeed_validated_s::SOURCE_SYNTHETIC) {
+
+			_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
+		}
+	}
+
+	// no airspeed updates for one second --> declare invalid
+	// this flag is used for some logic like: exiting takeoff, flaps retraction
+	_airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s;
+}
+
+void
+FixedWingGuidanceControl::wind_poll(const hrt_abstime now)
+{
+	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 = now;
+
+		_wind_vel(0) = wind.windspeed_north;
+		_wind_vel(1) = wind.windspeed_east;
+
+	} else {
+		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
+		_wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT;
+	}
+
+	if (!_wind_valid) {
+		_wind_vel(0) = 0.f;
+		_wind_vel(1) = 0.f;
+	}
+}
+
+void
+FixedWingGuidanceControl::manual_control_setpoint_poll()
+{
+	_sticks.checkAndUpdateStickInputs();
+
+	_manual_control_setpoint_for_height_rate = _sticks.getPitch();
+	_manual_control_setpoint_for_airspeed = _sticks.getThrottleZeroCentered();
+
+	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
+		/* Alternate stick allocation (similar concept as for multirotor systems:
+		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
+		 */
+
+		_manual_control_setpoint_for_height_rate = -_sticks.getThrottleZeroCentered();
+		_manual_control_setpoint_for_airspeed = _sticks.getPitch();
+	}
+}
+
+void
+FixedWingGuidanceControl::vehicle_attitude_poll()
+{
+	vehicle_attitude_s vehicle_attitude;
+
+	if (_vehicle_attitude_sub.update(&vehicle_attitude)) {
+		vehicle_angular_velocity_s angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&angular_velocity);
+		const Vector3f rates{angular_velocity.xyz};
+
+		Dcmf R{Quatf(vehicle_attitude.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.is_vtol_tailsitter) {
+			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
+			R = R * R_offset;
+
+			_yawrate = rates(0);
+
+		} else {
+			_yawrate = rates(2);
+		}
+
+		const Eulerf euler_angles(R);
+		_yaw = euler_angles(2);
+
+		const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
+		_body_acceleration_x = body_acceleration(0);
+
+		const Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
+		_body_velocity_x = body_velocity(0);
+	}
+}
+
+void
+FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now)
+{
+	/* reset flag when airplane landed */
+	if (_landed) {
+		_completed_manual_takeoff = false;
+	}
+}
+
+void
+FixedWingGuidanceControl::control_auto_path(const float control_interval,
+		const Vector2f &ground_speed, const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt,
+		const Vector2f velocity_2d, bool gliding_enabled)
+{
+	const float target_airspeed = cruising_speed > FLT_EPSILON ? cruising_speed : NAN;
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+
+	// Navigate directly on position setpoint and path tangent
+	const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 /
+				_pos_sp_triplet.current.loiter_radius :
+				0.0f;
+	const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(),
+					     ground_speed, _wind_vel, curvature);
+
+	fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
+	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
+	fw_lateral_ctrl_sp.course = sp.course_setpoint;
+	fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward;
+	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
+
+	const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
+		.timestamp = hrt_absolute_time(),
+		.altitude = curr_wp_alt,
+		.height_rate = NAN,
+		.equivalent_airspeed = target_airspeed,
+		.pitch_direct = NAN,
+		.throttle_direct = NAN
+	};
+
+	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
+
+	if (gliding_enabled) {
+		_ctrl_configuration_handler.setThrottleMin(0.0f);
+		_ctrl_configuration_handler.setThrottleMax(0.0f);
+		_ctrl_configuration_handler.setSpeedWeight(2.0f);
+	}
+}
+
+void
+FixedWingGuidanceControl::Run()
+{
+	if (should_exit()) {
+		_local_pos_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	_vehicle_status_sub.update(&_vehicle_status);
+
+	/* only run controller if position changed and we are not running an external mode*/
+
+	const bool is_external_nav_state = ((_vehicle_status.nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
+					    && (_vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
+
+	if (!is_external_nav_state) {
+		// this will cause the configuration handler to publish immediately the next time an internal flight
+		// mode is active
+		_ctrl_configuration_handler.resetLastPublishTime();
+
+	} else if (_local_pos_sub.update(&_local_pos)) {
+
+		const hrt_abstime now = _local_pos.timestamp;
+
+		const float control_interval = math::constrain((now - _last_time_position_control_called) * 1e-6f,
+					       MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
+		_last_time_position_control_called = now;
+
+		// 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();
+		}
+
+		vehicle_global_position_s gpos;
+
+		airspeed_poll();
+		manual_control_setpoint_poll();
+		vehicle_attitude_poll();
+		vehicle_control_mode_poll();
+		wind_poll(now);
+
+
+		if (_global_pos_sub.update(&gpos)) {
+			_current_latitude = gpos.lat;
+			_current_longitude = gpos.lon;
+		}
+
+		if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
+			_reference_altitude = _local_pos.ref_alt;
+
+		} else {
+			_reference_altitude = 0.f;
+		}
+
+		_current_altitude = -_local_pos.z + _reference_altitude; // Altitude AMSL in meters
+
+		// handle estimator reset events. we only adjust setpoins for manual modes
+		if (_control_mode.flag_control_manual_enabled) {
+			// adjust navigation waypoints in position control mode
+			if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
+			    && _local_pos.xy_reset_counter != _xy_reset_counter) {
+
+				// reset heading hold flag, which will re-initialise position control
+				_hdg_hold_enabled = false;
+			}
+		}
+
+		// Convert Local setpoints to global setpoints
+		if (!_global_local_proj_ref.isInitialized()
+		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
+		    || (_local_pos.xy_reset_counter != _xy_reset_counter)) {
+
+			double reference_latitude = 0.;
+			double reference_longitude = 0.;
+
+			if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
+				reference_latitude = _local_pos.ref_lat;
+				reference_longitude = _local_pos.ref_lon;
+			}
+
+			_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
+							     _local_pos.ref_timestamp);
+		}
+
+		if (_control_mode.flag_control_offboard_enabled) {
+			global_trajectory_setpoint_s global_trajectory_setpoint;
+
+			if (_global_trajectory_setpoint_sub.update(&global_trajectory_setpoint)) {
+				bool valid_setpoint = false;
+				_pos_sp_triplet = {}; // clear any existing
+				_pos_sp_triplet.timestamp = global_trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.timestamp = global_trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
+				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
+				_pos_sp_triplet.current.vx = NAN;
+				_pos_sp_triplet.current.vy = NAN;
+				_pos_sp_triplet.current.vz = NAN;
+				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
+				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
+				_pos_sp_triplet.current.alt = NAN;
+
+				if (PX4_ISFINITE(global_trajectory_setpoint.lat) && PX4_ISFINITE(global_trajectory_setpoint.lon)
+				    && PX4_ISFINITE(global_trajectory_setpoint.alt)) {
+					valid_setpoint = true;
+					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+					_pos_sp_triplet.current.lat = global_trajectory_setpoint.lat;
+					_pos_sp_triplet.current.lon = global_trajectory_setpoint.lon;
+					_pos_sp_triplet.current.alt = global_trajectory_setpoint.alt;
+				}
+
+				if (Vector3f(global_trajectory_setpoint.velocity).isAllFinite()) {
+					valid_setpoint = true;
+					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+					_pos_sp_triplet.current.vx = global_trajectory_setpoint.velocity[0];
+					_pos_sp_triplet.current.vy = global_trajectory_setpoint.velocity[1];
+					_pos_sp_triplet.current.vz = global_trajectory_setpoint.velocity[2];
+
+					if (Vector3f(global_trajectory_setpoint.acceleration).isAllFinite()) {
+						Vector2f velocity_sp_2d(global_trajectory_setpoint.velocity[0], global_trajectory_setpoint.velocity[1]);
+						Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
+						Vector2f acceleration_sp_2d(global_trajectory_setpoint.acceleration[0], global_trajectory_setpoint.acceleration[1]);
+						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) *
+									       normalized_velocity_sp_2d;
+						float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized());
+						_pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() /
+											acceleration_normal.norm();
+
+					} else {
+						_pos_sp_triplet.current.loiter_radius = NAN;
+					}
+				}
+
+				_position_setpoint_current_valid = valid_setpoint;
+			}
+		}
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+			}
+		}
+
+		if (!_vehicle_status.in_transition_mode) {
+			// reset position of backtransition start if not in transition
+			_lpos_where_backtrans_started = Vector2f(NAN, NAN);
+			_backtrans_heading = NAN;
+		}
+
+
+		Vector2d curr_pos(_current_latitude, _current_longitude);
+		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
+
+		update_in_air_states(now);
+
+		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
+
+		// restore lateral-directional guidance parameters (changed in takeoff mode)
+		_directional_guidance.setPeriod(_param_npfg_period.get());
+
+		// by default set speed weight to the param value, can be overwritten inside the methods below
+		_ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get());
+
+		Vector2f curr_wp_local = _global_local_proj_ref.project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
+		const matrix::Vector2f velocity_2d(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy);
+		control_auto_path(control_interval, ground_speed, _pos_sp_triplet.current.cruising_speed, curr_wp_local, _pos_sp_triplet.current.alt,
+				  velocity_2d,
+				  _pos_sp_triplet.current.gliding_enabled);
+
+		_ctrl_configuration_handler.update(now);
+
+		// only publish status in full FW mode
+		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+		    || _vehicle_status.in_transition_mode) {
+			publish_lateral_guidance_status(now);
+
+		}
+
+		_xy_reset_counter = _local_pos.xy_reset_counter;
+
+		perf_end(_loop_perf);
+	}
+}
+
+
+DirectionalGuidanceOutput FixedWingGuidanceControl::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)
+{
+	if (tangent_setpoint.norm() <= FLT_EPSILON) {
+		// degenerate case: no direction. maintain the last npfg command.
+		return DirectionalGuidanceOutput{};
+	}
+
+	const Vector2f unit_path_tangent{tangent_setpoint.normalized()};
+	_closest_point_on_path = position_setpoint;
+	return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(),
+			position_setpoint,
+			curvature);
+}
+
+void FixedWingGuidanceControl::publish_lateral_guidance_status(const hrt_abstime now)
+{
+	fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{};
+
+	fixed_wing_lateral_guidance_status.timestamp = now;
+	fixed_wing_lateral_guidance_status.course_setpoint = _directional_guidance.getCourseSetpoint();
+	fixed_wing_lateral_guidance_status.lateral_acceleration_ff = _directional_guidance.getLateralAccelerationSetpoint();
+	fixed_wing_lateral_guidance_status.bearing_feas = _directional_guidance.getBearingFeasibility();
+	fixed_wing_lateral_guidance_status.bearing_feas_on_track = _directional_guidance.getBearingFeasibilityOnTrack();
+	fixed_wing_lateral_guidance_status.signed_track_error = _directional_guidance.getSignedTrackError();
+	fixed_wing_lateral_guidance_status.track_error_bound = _directional_guidance.getTrackErrorBound();
+	fixed_wing_lateral_guidance_status.adapted_period = _directional_guidance.getAdaptedPeriod();
+	fixed_wing_lateral_guidance_status.wind_est_valid = _wind_valid;
+
+	_fixed_wing_lateral_guidance_status_pub.publish(fixed_wing_lateral_guidance_status);
+}
+
+int FixedWingGuidanceControl::task_spawn(int argc, char *argv[])
+{
+	FixedWingGuidanceControl *instance = new FixedWingGuidanceControl();
+
+	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;
+}
+
+int FixedWingGuidanceControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FixedWingGuidanceControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the setpoint generation for all PX4-internal fixed-wing modes, height-rate control and higher.
+It takes the current mode state of the vehicle as input and outputs setpoints consumed by the fixed-wing
+lateral-longitudinal controller and and controllers below that (attitude, rate).
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_mode_manager", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fw_guidance_control_main(int argc, char *argv[])
+{
+	return FixedWingGuidanceControl::main(argc, argv);
+}
@@ -0,0 +1,428 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 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 FixedWingGuidanceControl.hpp
+ * Implementation of various fixed-wing control modes.
+ */
+
+#ifndef FIXEDWINGGUIDANCECONTROL_HPP_
+#define FIXEDWINGGUIDANCECONTROL_HPP_
+
+#include "ControllerConfigurationHandler.hpp"
+
+#include <float.h>
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
+#include <lib/npfg/DirectionalGuidance.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/sticks/Sticks.hpp>
+#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/uORB.h>
+
+#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/fixed_wing_lateral_setpoint.h>
+#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
+#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
+#include <uORB/topics/fixed_wing_runway_control.h>
+#include <uORB/topics/launch_detection_status.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/global_trajectory_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
+#include <uORB/topics/orbit_status.h>
+
+using namespace time_literals;
+
+using matrix::Vector2d;
+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;
+
+// [rad/s] max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
+
+// [.] max manual roll/yaw normalized input from user which does not change the locked heading
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
+
+// [us] time after which we abort landing if terrain estimate is not valid. this timer start whenever the terrain altitude
+// was previously valid, and has changed to invalid.
+static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
+
+// [us] within this timeout, if a distance sensor measurement not yet made, the land waypoint altitude is used for terrain
+// altitude. this timer starts at the beginning of the landing glide slope.
+static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
+
+// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float THROTTLE_THRESH = -.9f;
+
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
+
+// [s] minimum time step between auto control updates
+static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
+
+// [s] maximum time step between auto control updates
+static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
+
+// [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
+static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
+
+// [m] arbitrary buffer altitude added to clearance altitude setpoint during takeoff to ensure aircraft passes the clearance
+// altitude while waiting for navigator to flag it exceeded
+static constexpr float kClearanceAltitudeBuffer = 10.0f;
+
+// [m/s] maximum rate at which the touchdown position can be nudged
+static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f;
+
+// [.] normalized deadzone threshold for manual nudging input
+static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
+
+// [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare)
+static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
+
+// [] Stick deadzon
+static constexpr float kStickDeadBand = 0.06f;
+
+class FixedWingGuidanceControl final : public ModuleBase<FixedWingGuidanceControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FixedWingGuidanceControl();
+	~FixedWingGuidanceControl() 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::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _global_trajectory_setpoint_sub{ORB_ID(global_trajectory_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
+	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::PublicationData<fixed_wing_lateral_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)};
+	uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
+	uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
+
+	position_setpoint_triplet_s _pos_sp_triplet{};
+	vehicle_control_mode_s _control_mode{};
+	vehicle_local_position_s _local_pos{};
+	vehicle_status_s _vehicle_status{};
+
+	CombinedControllerConfigurationHandler _ctrl_configuration_handler;
+
+	Vector2f _lpos_where_backtrans_started;
+
+	bool _position_setpoint_previous_valid{false};
+	bool _position_setpoint_current_valid{false};
+	bool _position_setpoint_next_valid{false};
+
+	perf_counter_t _loop_perf; // loop performance counter
+
+	// [us] Last absolute time position control has been called
+	hrt_abstime _last_time_position_control_called{0};
+
+	uint8_t _position_sp_type{0};
+
+	enum StickConfig {
+		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
+		STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
+	};
+
+
+	Sticks _sticks{this};
+
+	// VEHICLE STATES
+
+	double _current_latitude{0};
+	double _current_longitude{0};
+	float _current_altitude{0.f};
+
+	float _yaw{0.0f};
+	float _yawrate{0.0f};
+
+	float _body_acceleration_x{0.f};
+	float _body_velocity_x{0.f};
+
+	MapProjection _global_local_proj_ref{};
+
+	float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
+
+	bool _landed{true};
+
+	// MANUAL MODES
+
+	// indicates whether we have completed a manual takeoff in a position control mode
+	bool _completed_manual_takeoff{false};
+
+	// [rad] yaw setpoint for manual position mode heading hold
+	float _hdg_hold_yaw{0.0f};
+
+	bool _hdg_hold_enabled{false}; // heading hold enabled
+	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
+
+	Vector2f _hdg_hold_position{}; // position where heading hold started
+
+	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
+	float _manual_control_setpoint_for_height_rate{0.0f};
+
+	// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
+	float _manual_control_setpoint_for_airspeed{0.0f};
+
+	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	float _commanded_manual_airspeed_setpoint{NAN};
+
+	// AUTO TAKEOFF
+
+	// [m] ground altitude AMSL where the plane was launched
+	float _takeoff_ground_alt{0.0f};
+
+	// true if a launch, specifically using the launch detector, has been detected
+	bool _launch_detected{false};
+
+	// [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway)
+	Vector2d _takeoff_init_position{0, 0};
+
+	// [rad] current vehicle yaw at the time the launch is detected
+	float _launch_current_yaw{0.f};
+
+	bool _skipping_takeoff_detection{false};
+
+	// AUTO LANDING
+	// [m] lateral touchdown position offset manually commanded during landing
+	float _lateral_touchdown_position_offset{0.0f};
+
+	// [m] last terrain estimate which was valid
+	float _last_valid_terrain_alt_estimate{0.0f};
+
+	// [us] time at which we had last valid terrain alt
+	hrt_abstime _last_time_terrain_alt_was_valid{0};
+
+	// AIRSPEED
+
+	float _airspeed_eas{0.f};
+	bool _airspeed_valid{false};
+
+	// [us] last time airspeed was received. used to detect timeouts.
+	hrt_abstime _time_airspeed_last_valid{0};
+
+	// WIND
+
+	// [m/s] wind velocity vector
+	Vector2f _wind_vel{0.0f, 0.0f};
+
+	bool _wind_valid{false};
+
+	hrt_abstime _time_wind_last_received{0}; // [us]
+
+	// VTOL / TRANSITION
+	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};
+	uint64_t _time_last_xy_reset{0};
+
+	// LATERAL-DIRECTIONAL GUIDANCE
+
+	// CLosest point on path to track
+	matrix::Vector2f _closest_point_on_path;
+
+	// nonlinear path following guidance - lateral-directional position control
+	DirectionalGuidance _directional_guidance;
+
+	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
+	float _min_current_sp_distance_xy{FLT_MAX};
+
+	// Update our local parameter cache.
+	void parameters_update();
+
+	// Update subscriptions
+	void airspeed_poll();
+
+	void manual_control_setpoint_poll();
+	void vehicle_attitude_poll();
+	void vehicle_control_mode_poll();
+
+	void wind_poll(const hrt_abstime now);
+
+	/**
+	 * @brief Vehicle control for following a path.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_path(const float control_interval, const Vector2f &ground_speed,
+			       const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt, const Vector2f velocity_2d, bool gliding_enabled);
+
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+
+	/**
+	 * @brief Updates timing information for landed and in-air states.
+	 *
+	 * @param now Current system time [us]
+	 */
+	void update_in_air_states(const hrt_abstime now);
+
+	/*
+	 * Path following logic. Takes poisiton, path tangent, curvature and
+	 * then updates control setpoints to follow a path setpoint.
+	 *
+	 * TODO: deprecate this function with a proper API to NPFG.
+	 *
+	 * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m]
+	 * @param[in] position_setpoint closest point on a path in local coordinates. (N,E) [m]
+	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] curvature of the path setpoint [1/m]
+	 */
+	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);
+
+	void publish_lateral_guidance_status(const hrt_abstime now);
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
+
+		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
+		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
+		(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
+		(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
+		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
+		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
+		(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
+
+		(ParamFloat<px4::params::FW_LND_AIRSPD>) _param_fw_lnd_airspd,
+		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
+		(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,
+		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
+		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
+
+		(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_CLMB_R_SP>) _param_climbrate_target,
+		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
+		(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_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
+		(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
+		(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
+		(ParamInt<px4::params::FW_POS_STK_CONF>) _param_fw_pos_stk_conf,
+		(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
+		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_t_spdweight,
+
+		// external parameters
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
+		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
+		(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
+		(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
+		(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
+		(ParamFloat<px4::params::FW_LND_TD_TIME>) _param_fw_lnd_td_time,
+		(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
+		(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
+		(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
+		(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
+		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
+		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
+		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
+		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
+		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max
+	)
+};
+
+#endif // FIXEDWINGGUIDANCECONTROL_HPP_
@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_GUIDANCE_CONTROL
+	bool "fw_guidance_control"
+	default n
+	---help---
+		Enable support for fw_guidance_control
+
+menuconfig USER_FW_MODE_MANAGER
+	bool "fw_guidance_control running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_MODE_MANAGER
+	---help---
+		Put fw_guidance_control in userspace memory
@@ -0,0 +1,33 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 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.
+ *
+ ****************************************************************************/
+
@@ -141,11 +141,12 @@ void FwLateralLongitudinalControl::Run()

 	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;
+		const hrt_abstime now = _local_pos.timestamp_sample;

-		updateControllerConfiguration();
+		const float control_interval = math::constrain((now - _last_time_loop_ran) * 1e-6f, 0.001f, 0.1f);
+		_last_time_loop_ran = now;
+
+		updateControllerConfiguration(now);

 		_tecs.set_speed_weight(_long_configuration.speed_weight);
 		updateTECSAltitudeTimeConstant(checkLowHeightConditions()
@@ -165,8 +166,6 @@ void FwLateralLongitudinalControl::Run()
 			_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();

@@ -176,7 +175,7 @@ void FwLateralLongitudinalControl::Run()
 			_flaps_setpoint = flaps_setpoint.normalized_setpoint;
 		}

-		update_control_state();
+		update_control_state(now);

 		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) {
@@ -219,7 +218,8 @@ void FwLateralLongitudinalControl::Run()
 						   _long_configuration.sink_rate_target,
 						   _long_configuration.climb_rate_target,
 						   _long_configuration.disable_underspeed_protection,
-						   _long_control_sp.height_rate
+						   _long_control_sp.height_rate,
+						   now
 						  );

 			pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
@@ -279,13 +279,13 @@ void FwLateralLongitudinalControl::Run()
 				lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration
 			}

-			lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
+			lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp, now);
 			lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max,
 							   _lateral_configuration.lateral_accel_max);
 			roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);

 			fixed_wing_lateral_status_s fixed_wing_lateral_status{};
-			fixed_wing_lateral_status.timestamp = hrt_absolute_time();
+			fixed_wing_lateral_status.timestamp = now;
 			fixed_wing_lateral_status.lateral_acceleration_setpoint = lateral_accel_sp;
 			fixed_wing_lateral_status.can_run_factor = _can_run_factor;

@@ -307,7 +307,7 @@ void FwLateralLongitudinalControl::Run()
 			// roll slew rate
 			roll_body = _roll_slew_rate.update(roll_body, control_interval);

-			_att_sp.timestamp = hrt_absolute_time();
+			_att_sp.timestamp = now;
 			const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 			q.copyTo(_att_sp.q_d);

@@ -323,16 +323,16 @@ void FwLateralLongitudinalControl::Run()
 	perf_end(_loop_perf);
 }

-void FwLateralLongitudinalControl::updateControllerConfiguration()
+void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime timestamp)
 {
 	if (_lateral_configuration.timestamp == 0) {
-		_lateral_configuration.timestamp = _local_pos.timestamp;
+		_lateral_configuration.timestamp = timestamp;
 		_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;

 	}

 	if (_long_configuration.timestamp == 0) {
-		setDefaultLongitudinalControlConfiguration();
+		setDefaultLongitudinalControlConfiguration(timestamp);
 	}

 	if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
@@ -361,7 +361,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 		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)
+		bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now)
 {
 	bool tecs_is_running = true;

@@ -400,7 +400,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 		     _long_control_state.height_rate,
 		     hgt_rate_sp);

-	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated, now);

 	if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
 	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
@@ -420,14 +420,19 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 			_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
+			// We can't infer the flight phase , do nothing, estimation is reset at each step
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
+
 		}
+
+		_flight_phase_estimation_pub.get().timestamp = now;
+		_flight_phase_estimation_pub.update();
 	}
 }

 void
 FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
-		float true_airspeed_derivative_raw, float throttle_trim)
+		float true_airspeed_derivative_raw, float throttle_trim, hrt_abstime timestamp)
 {
 	tecs_status_s tecs_status{};

@@ -458,7 +463,7 @@ FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent
 	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.timestamp = timestamp;

 	_tecs_status_pub.publish(tecs_status);
 }
@@ -531,16 +536,16 @@ fw_lat_lon_control computes attitude and throttle setpoints from lateral and lon
 	return 0;
 }

-void FwLateralLongitudinalControl::update_control_state() {
+void FwLateralLongitudinalControl::update_control_state(hrt_abstime now) {
 	updateAltitudeAndHeightRate();
 	updateAirspeed();
 	updateAttitude();
-	updateWind();
+	updateWind(now);

 	_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
 }

-void FwLateralLongitudinalControl::updateWind() {
+void FwLateralLongitudinalControl::updateWind(hrt_abstime now) {
 	if (_wind_sub.updated()) {
 		wind_s wind{};
 		_wind_sub.update(&wind);
@@ -549,14 +554,14 @@ void FwLateralLongitudinalControl::updateWind() {
 		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
 			      && PX4_ISFINITE(wind.windspeed_east);

-		_time_wind_last_received = hrt_absolute_time();
+		_time_wind_last_received = now;

 		_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;
+		_wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT;
 	}

 	if (!_wind_valid) {
@@ -735,32 +740,30 @@ float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local

 	return flying_forward_factor * low_ground_speed_factor;
 }
-float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
+float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now)
 {
 	// Scale the npfg output to zero if npfg is not certain for correct output
 	_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) {
+	if ((now - _time_of_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
 		_need_report_npfg_uncertain_condition = true;
-		_time_since_first_reduced_roll = 0U;
+		_time_of_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;
+		_time_of_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 (_time_of_first_reduced_roll == 0U) {
+			_time_of_first_reduced_roll = now;
 		}

-		if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
+		if ((now - _time_of_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!");
@@ -770,7 +773,7 @@ float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float later
 		// Nothing to do, already reported.
 	}

-	_time_since_last_npfg_call = now;
+	_time_of_last_npfg_call = now;

 	return _can_run_factor * (lateral_accel_sp);
 }
@@ -778,8 +781,8 @@ float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float late
 	return  atanf(lateral_acceleration_sp / CONSTANTS_ONE_G);
 }

-void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() {
-	_long_configuration.timestamp = hrt_absolute_time();
+void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration(hrt_abstime timestamp) {
+	_long_configuration.timestamp = timestamp;
 	_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
 	_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
 	_long_configuration.throttle_min = _param_fw_thr_min.get();
@@ -195,8 +195,8 @@ private:
 		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
+	hrt_abstime _time_of_first_reduced_roll{0U};       ///< absolute time when entering reduced roll angle for the first time
+	hrt_abstime _time_of_last_npfg_call{0U};           ///< absolute time when the npfg reduced roll angle calculations was last performed
 	vehicle_attitude_setpoint_s _att_sp{};
 	bool _landed{false};
 	float _can_run_factor{0.f};
@@ -212,15 +212,15 @@ private:
 	float _min_airspeed_from_guidance{0.f}; // need to store it bc we only update after running longitudinal controller

 	void parameters_update();
-	void update_control_state();
+	void update_control_state(hrt_abstime now);
 	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);
+					bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);

 	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
-				 float throttle_trim);
+				 float throttle_trim, hrt_abstime now);

 	void updateAirspeed();

@@ -230,7 +230,7 @@ private:

 	float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const;

-	void updateWind();
+	void updateWind(hrt_abstime now);

 	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);

@@ -238,13 +238,13 @@ private:

 	float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const;

-	float getCorrectedLateralAccelSetpoint(float lateral_accel_sp);
+	float getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now);

-	void setDefaultLongitudinalControlConfiguration();
+	void setDefaultLongitudinalControlConfiguration(hrt_abstime now);

 	void updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in);

-	void updateControllerConfiguration();
+	void updateControllerConfiguration(hrt_abstime now);

 	float getLoadFactor() const;

@@ -373,26 +373,10 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 	_skipping_takeoff_detection = false;
 	const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;

-	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid
-	    && _control_mode.flag_control_position_enabled) {
-		if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
-		    && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
-			// Offboard position with velocity setpoints
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
-			return;
+	if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
+	    && _position_setpoint_current_valid) {

-		} else {
-			// Offboard position setpoint only
-			_control_mode_current = FW_POSCTRL_MODE_AUTO;
-			return;
-		}
-
-	} else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-		   && (_position_setpoint_current_valid
-		       || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
-
-		// Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
-		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
+		// Enter this mode only if the current waypoint has valid 3D position setpoints.

 		if (doing_backtransition) {
 			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW;
@@ -433,6 +417,28 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 			_control_mode_current = FW_POSCTRL_MODE_AUTO;
 		}

+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
+		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+
+		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
+		if (doing_backtransition) {
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
+
+		} else {
+			_control_mode_current = FW_POSCTRL_MODE_AUTO;
+		}
+
+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
+		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
+
+		if (doing_backtransition) {
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
+
+		} else {
+			// Only circular landing are supported when LAND is sent without valid position
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
+		}
+
 	} else if (_control_mode.flag_control_auto_enabled
 		   && _control_mode.flag_control_climb_rate_enabled
 		   && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes
@@ -1013,47 +1019,6 @@ void FixedWingModeManager::publishFigureEightStatus(const position_setpoint_s po
 }
 #endif // CONFIG_FIGURE_OF_EIGHT

-void
-FixedWingModeManager::control_auto_path(const float control_interval, const Vector2d &curr_pos,
-					const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
-{
-	const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
-
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
-
-	// Navigate directly on position setpoint and path tangent
-	const matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-	const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 /
-				_pos_sp_triplet.current.loiter_radius :
-				0.0f;
-	const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(),
-					     ground_speed, _wind_vel, curvature);
-
-	fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
-	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
-	fw_lateral_ctrl_sp.course = sp.course_setpoint;
-	fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward;
-	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
-
-	const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
-		.timestamp = hrt_absolute_time(),
-		.altitude = pos_sp_curr.alt,
-		.height_rate = NAN,
-		.equivalent_airspeed = target_airspeed,
-		.pitch_direct = NAN,
-		.throttle_direct = NAN
-	};
-
-	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
-
-	if (pos_sp_curr.gliding_enabled) {
-		_ctrl_configuration_handler.setThrottleMin(0.0f);
-		_ctrl_configuration_handler.setThrottleMax(0.0f);
-		_ctrl_configuration_handler.setSpeedWeight(2.0f);
-	}
-}
-
 void
 FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
 		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
@@ -1594,6 +1559,12 @@ void
 FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, const float control_interval,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
+	if (_time_started_landing == 0) {
+		// save time at which we started landing and reset landing abort status
+		reset_landing_state();
+		_time_started_landing = now;
+	}
+
 	const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
 				    _param_fw_airspd_min.get();

@@ -1601,12 +1572,11 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons


 	const Vector2f local_position{_local_pos.x, _local_pos.y};
-	Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);

-	if (_time_started_landing == 0) {
-		// save time at which we started landing and reset landing abort status
-		reset_landing_state();
-		_time_started_landing = now;
+	if (!_local_landing_orbit_center.isAllFinite()) {
+		_local_landing_orbit_center = _position_setpoint_current_valid
+					      ? _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon)
+					      : local_position;
 	}

 	const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
@@ -1642,7 +1612,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 						      1.0f);

 		/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */
-		const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
+		const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius,
 						     loiter_direction_ccw,
 						     ground_speed, _wind_vel);
 		fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
@@ -1700,7 +1670,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 	} else {

 		// follow the glide slope
-		const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
+		const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius,
 						     loiter_direction_ccw,
 						     ground_speed, _wind_vel);
 		fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
@@ -2027,80 +1997,22 @@ FixedWingModeManager::Run()
 							     _local_pos.ref_timestamp);
 		}

-		if (_control_mode.flag_control_offboard_enabled) {
-			trajectory_setpoint_s trajectory_setpoint;
+		if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {

-			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
-				bool valid_setpoint = false;
-				_pos_sp_triplet = {}; // clear any existing
-				_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-				_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
-				_pos_sp_triplet.current.vx = NAN;
-				_pos_sp_triplet.current.vy = NAN;
-				_pos_sp_triplet.current.vz = NAN;
-				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
-				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
-				_pos_sp_triplet.current.alt = NAN;
+			_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
+							    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
+							    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);

-				if (Vector3f(trajectory_setpoint.position).isAllFinite()) {
-					if (_global_local_proj_ref.isInitialized()) {
-						double lat;
-						double lon;
-						_global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon);
-						valid_setpoint = true;
-						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-						_pos_sp_triplet.current.lat = lat;
-						_pos_sp_triplet.current.lon = lon;
-						_pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2];
-					}
+			_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
+							   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
+							   && PX4_ISFINITE(_pos_sp_triplet.current.alt);

-				}
+			_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
+							&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
+							&& PX4_ISFINITE(_pos_sp_triplet.next.alt);

-				if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) {
-					valid_setpoint = true;
-					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-					_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
-					_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
-					_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
-
-					if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) {
-						Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
-						Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
-						Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
-						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) *
-									       normalized_velocity_sp_2d;
-						float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized());
-						_pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() /
-											acceleration_normal.norm();
-
-					} else {
-						_pos_sp_triplet.current.loiter_radius = NAN;
-					}
-				}
-
-				_position_setpoint_current_valid = valid_setpoint;
-			}
-
-		} else {
-			if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
-
-				_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
-								    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
-								    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
-
-				_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
-								   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
-								   && PX4_ISFINITE(_pos_sp_triplet.current.alt);
-
-				_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
-								&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
-								&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
-
-				// reset the altitude foh (first order hold) logic
-				_min_current_sp_distance_xy = FLT_MAX;
-			}
+			// reset the altitude foh (first order hold) logic
+			_min_current_sp_distance_xy = FLT_MAX;
 		}

 		airspeed_poll();
@@ -2185,11 +2097,6 @@ FixedWingModeManager::Run()
 				break;
 			}

-		case FW_POSCTRL_MODE_AUTO_PATH: {
-				control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
-				break;
-			}
-
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
 				control_auto_takeoff(now, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;
@@ -2285,6 +2192,7 @@ FixedWingModeManager::reset_landing_state()

 	_flare_states = FlareStates{};

+	_local_landing_orbit_center.setNaN();
 	_lateral_touchdown_position_offset = 0.0f;

 	_last_time_terrain_alt_was_valid = 0;
@@ -2299,6 +2207,10 @@ FixedWingModeManager::reset_landing_state()

 float FixedWingModeManager::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const
 {
+	if (!PX4_ISFINITE(altitude) || !PX4_ISFINITE(terrain_altitude)) {
+		return math::radians(_param_fw_r_lim.get());
+	}
+
 	// we want the wings level when at the wing height above ground
 	const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);

@@ -2424,7 +2336,7 @@ FixedWingModeManager::getLandingTerrainAltitudeEstimate(const hrt_abstime &now,

 		if (_local_pos.dist_bottom_valid) {

-			const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom;
+			const float terrain_estimate = _reference_altitude + -_local_pos.z - _local_pos.dist_bottom;
 			_last_valid_terrain_alt_estimate = terrain_estimate;
 			_last_time_terrain_alt_was_valid = now;

@@ -77,7 +77,6 @@
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -181,7 +180,6 @@ private:
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
@@ -228,7 +226,6 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
-		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW,
@@ -319,6 +316,7 @@ private:
 	// orbit to altitude only when the aircraft has entered the final *straight approach.
 	hrt_abstime _time_started_landing{0};

+	Vector2f _local_landing_orbit_center{NAN, NAN};
 	// [m] lateral touchdown position offset manually commanded during landing
 	float _lateral_touchdown_position_offset{0.0f};

@@ -1115,6 +1115,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
 		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
 		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.attitude = PX4_ISFINITE(setpoint.yaw);
+		ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);

 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@@ -1237,6 +1239,8 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
 		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
 		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.attitude = PX4_ISFINITE(setpoint.yaw);
+		ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);

 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");
@@ -408,6 +408,7 @@ void MulticopterPositionControl::Run()
 				} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
 					// clear existing setpoint when controller is no longer active
 					_setpoint = PositionControl::empty_trajectory_setpoint;
+					_control.setInputSetpoint(_setpoint);
 				}
 			}
 		}
@@ -84,8 +84,11 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new)
 	const float previous_hover_thrust = _hover_thrust;
 	setHoverThrust(hover_thrust_new);

-	_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust
-		       + CONSTANTS_ONE_G - _acc_sp(2);
+	if (PX4_ISFINITE(_acc_sp(2))) {
+		_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust
+			       + CONSTANTS_ONE_G - _acc_sp(2);
+	}
+
 }

 void PositionControl::setState(const PositionControlStates &states)
@@ -76,6 +76,11 @@ void MecanumAttControl::updateAttControl()
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
 		_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
+		_last_yaw_setpoint_timestamp = hrt_absolute_time();
+	}
+
+	if (hrt_elapsed_time(&_last_yaw_setpoint_timestamp) > YAW_SETPOINT_TIMEOUT_US) {
+		_yaw_setpoint = NAN;
 	}

 	if (PX4_ISFINITE(_yaw_setpoint)) {
@@ -34,6 +34,7 @@
 #pragma once

 // PX4 includes
+#include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/events.h>

@@ -52,6 +53,8 @@
 #include <uORB/topics/rover_attitude_status.h>
 #include <uORB/topics/rover_attitude_setpoint.h>

+using namespace time_literals;
+
 /**
 * @brief Class for mecanum attitude control.
 */
@@ -103,6 +106,10 @@ private:
 	float _max_yaw_rate{0.f};
 	float _yaw_setpoint{NAN};

+	hrt_abstime _last_yaw_setpoint_timestamp{0};
+	/** Timeout in us for yaw setpoint to get considered invalid */
+	static constexpr uint64_t YAW_SETPOINT_TIMEOUT_US = 500_ms;
+
 	// Controllers
 	PID _pid_yaw;
 	SlewRateYaw<float> _adjusted_yaw_setpoint;
@@ -88,7 +88,10 @@ void MecanumOffboardMode::offboardControl()
 		rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
 		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);

-	} else if (offboard_control_mode.attitude) {
+	}
+
+	// For Mecanum wheel systems, attitude and position control can be decoupled
+	if (offboard_control_mode.attitude) {
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		rover_attitude_setpoint.timestamp = hrt_absolute_time();
 		rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
@@ -47,8 +47,6 @@ MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent
 void MecanumPosControl::updateParams()
 {
 	ModuleParams::updateParams();
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-
 }

 void MecanumPosControl::updatePosControl()
@@ -89,10 +87,6 @@ void MecanumPosControl::updatePosControl()
 			rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0);
 			rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1);
 			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = timestamp;
-			rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);

 		} else {
 			rover_speed_setpoint_s rover_speed_setpoint{};
@@ -100,10 +94,6 @@ void MecanumPosControl::updatePosControl()
 			rover_speed_setpoint.speed_body_x = 0.f;
 			rover_speed_setpoint.speed_body_y = 0.f;
 			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = timestamp;
-			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);

 			if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
 				_stopped = true;
@@ -124,7 +114,6 @@ void MecanumPosControl::updateSubscriptions()
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
 		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}

 	if (_vehicle_local_position_sub.updated()) {
@@ -114,9 +114,6 @@ private:
 	Vector2f _start_ned{};
 	Vector2f _target_waypoint_ned{};
 	float _arrival_speed{0.f};
-	float _vehicle_yaw{0.f};
-	float _max_yaw_rate{0.f};
-	float _yaw_setpoint{NAN};
 	float _vehicle_speed{0.f};
 	float _cruising_speed{NAN};
 	bool _stopped{false};
@@ -36,6 +36,7 @@
 #include <px4_platform_common/log.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
+#include <lib/drivers/device/Device.hpp>

 namespace sensors
 {
@@ -95,7 +96,12 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 		_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
 		_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
 		_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
-		_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
+
+		const int gps_prime = _param_sens_gps_prime.get();
+
+		if (math::isInRange(gps_prime, -1, 1)) {
+			_gps_blending.setPrimaryInstance(gps_prime);
+		}
 	}
 }

@@ -113,6 +119,7 @@ void VehicleGPSPosition::Run()
 	// Check all GPS instance
 	bool any_gps_updated = false;
 	bool gps_updated = false;
+	const int32_t gps_prime = _param_sens_gps_prime.get();

 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
 		gps_updated = _sensor_gps_sub[i].updated();
@@ -125,6 +132,16 @@ void VehicleGPSPosition::Run()
 			_sensor_gps_sub[i].copy(&gps_data);
 			_gps_blending.setGpsData(gps_data, i);

+			if (math::isInRange(static_cast<int>(gps_prime), 2, 127)) {
+				device::Device::DeviceId device_id{};
+				device_id.devid = gps_data.device_id;
+
+				if (device_id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN
+				    && device_id.devid_s.address == static_cast<uint8_t>(gps_prime)) {
+					_gps_blending.setPrimaryInstance(i);
+				}
+			}
+
 			if (!_sensor_gps_sub[i].registered()) {
 				_sensor_gps_sub[i].registerCallback();
 			}
@@ -70,13 +70,16 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
 * send data to the EKF even if a secondary instance is already available.
 * The secondary instance is then only used if the primary one times out.
 *
- * To have an equal priority of all the instances, set this parameter to -1 and
- * the best receiver will be used.
+ * Accepted values:
+ * -1 : Auto (equal priority for all instances)
+ * 0 : Main serial GPS instance
+ * 1 : Secondary serial GPS instance
+ * 2-127 : UAVCAN module node ID
 *
 * This parameter has no effect if blending is active.
 *
 * @group Sensors
 * @min -1
- * @max 1
+ * @max 127
 */
 PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);
@@ -160,6 +160,9 @@ subscriptions:
  - topic: /fmu/in/trajectory_setpoint
    type: px4_msgs::msg::TrajectorySetpoint

+  - topic: /fmu/in/global_trajectory_setpoint
+    type: px4_msgs::msg::GlobalTrajectorySetpoint
+
  - topic: /fmu/in/vehicle_attitude_setpoint
    type: px4_msgs::msg::VehicleAttitudeSetpoint

@@ -142,3 +142,14 @@ parameters:
            category: System
            reboot_required: true
            default: -1
+
+        UXRCE_DDS_FLCTRL:
+            description:
+                short: Enable serial flow control for UXRCE interface
+                long: |
+                    This is used to enable flow control for the serial uXRCE instance.
+                    Used for reliable high bandwidth communication.
+            type: boolean
+            category: System
+            reboot_required: true
+            default: 0
@@ -837,8 +837,16 @@ bool UxrceddsClient::setBaudrate(int fd, unsigned baud)
 	//
 	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);

-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	/* enable flow control if needed */
+	if (_param_uxrce_dds_flctrl.get() > 0) {
+		uart_config.c_cflag |= CRTSCTS;
+
+	} else {
+		uart_config.c_cflag &= ~CRTSCTS;
+	}

 	/* set baud rate */
 	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
@@ -210,6 +210,7 @@ private:
 		(ParamInt<px4::params::UXRCE_DDS_SYNCC>) _param_uxrce_dds_syncc,
 		(ParamInt<px4::params::UXRCE_DDS_SYNCT>) _param_uxrce_dds_synct,
 		(ParamInt<px4::params::UXRCE_DDS_TX_TO>) _param_uxrce_dds_tx_to,
-		(ParamInt<px4::params::UXRCE_DDS_RX_TO>) _param_uxrce_dds_rx_to
+		(ParamInt<px4::params::UXRCE_DDS_RX_TO>) _param_uxrce_dds_rx_to,
+		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl
 	)
 };
Author	SHA1	Message	Date
JaeyoungLim	19d9d55869	Add path api Fix	2026-03-16 07:16:59 -07:00
JaeyoungLim	a0b7e1427b	Offboard mode working	2026-03-14 17:54:48 -07:00
JaeyoungLim	5cc60834d2	Remove pos_sp_triplet	2026-03-14 17:05:45 -07:00
JaeyoungLim	a6a21bec45	Fix format Fix	2026-03-14 17:04:10 -07:00
JaeyoungLim	097e5b1904	Fix condition	2026-01-11 07:41:29 -08:00
JaeyoungLim	04b269eb5a	Add global trajectory setpoint message Integrate global path setpoint	2026-01-11 07:29:34 -08:00
JaeyoungLim	89113c8fff	Remove more stuff	2026-01-11 07:21:47 -08:00
JaeyoungLim	870cd34d17	Only run fixedwing guidance controller in external modes	2026-01-10 07:14:03 -08:00
JaeyoungLim	87574986d5	Remove current mode Remove landing logic Remove waypoint navigtion logic	2026-01-10 07:10:05 -08:00
JaeyoungLim	d656326e79	Add boiler plate FW guidance control boiler plate Remove modules Remove modules Add FWGuidanceControl Add KConfig Build guidance controller Make boiler plate compile Remove code Cleanup Remove trajectory setpoint F	2026-01-10 06:33:31 -08:00
Hamish Willee	cf50ecf41b	Split out PX4 v1.17 release note (#26225 )	2026-01-10 14:06:19 +11:00
Jaeyoung Lim	9fe69d4f33	Make flap slew rate configurable (#26240 ) * Make flap slew rate configurable * Dynamically update params	2026-01-09 06:37:34 -08:00
fakerror	f4247aee58	rover_mecanum: enable yaw control via MAVLink SET_POSITION_TARGET commands (#26218 ) * rover_mecanum: enable yaw control via MAVLink SET_POSITION_TARGET commands * Maintain the original judgment conditions --------- Co-authored-by: V <null>	2026-01-09 08:08:53 -05:00
Beniamino Pozzan	ec8f34325e	chore(rCS posix): move param override through env var after airframe selection (#26193 ) Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>	2026-01-09 12:15:45 +01:00
Matthias Grob	0e615816b3	commander/failsafe: fix user takeover not possible in fallback Land action when configured RTL is not possible	2026-01-09 10:10:23 +01:00
Matthias Grob	4cf4f82233	failsafe unit test: add cases for 1 allow taking over from degraded failsafes 2 not cause immediate takeover when failsafe happens because of mode switch The first test makes sure the user can take over when an RTL failsafe was triggered but degraded to a Land. The second test rules out the easiest fix of removing the condition `_selected_action == selected_action` which causes the problem for test one but is there for a reason.	2026-01-09 10:10:23 +01:00
mengchaoheng	5f83c186ee	Checks for identical parameter changes	2026-01-09 08:53:09 +01:00
Hamish Willee	e761297003	Fixed Wing Loiter Modes - Orbit and Figure 8 (#26194 )	2026-01-09 17:12:43 +11:00
Alex Klimaj	27181619fc	Update GPS submodule (#26238 )	2026-01-08 10:24:28 -09:00
Patrik Dominik Pordi	210239324d	Added ARK G5 and G5 Heading RTK GPSs (#26154 )	2026-01-09 05:59:09 +11:00
Mathieu Bresciani	c71e2d41d6	Fixedwing: Fix circular landing when global origin is not set (#26223 ) When not specified by navigator, the center of the landing orbit is set to the current position when landing is triggered.	2026-01-08 14:44:00 +01:00
mahima-yoga	7c318a3296	MulticopterPositionControl: prevent velocity integrator filling up from stale acceleration setpoints When position control is disabled, clear the setpoint properly to prevent stale values. This fixes a bug where switching to position mode in the same control loop as a hover thrust estimate update could fill up the velocity integrator.	2026-01-08 11:16:52 +01:00
Julian Oes	0375f1a6f0	airframes: don't mess with logging profile (#26234 ) I don't think we should change the logging profile based on the type of airframe configured. Instead, this is an option you set based on the phase of development/testing you're in. This came up because the KakuteH7v2 which is 4050 by default would log excessively which is not a good idea with only 128 MB flash storage.	2026-01-07 20:31:52 -09:00
Pascal	0ef4f1b7bd	Add uxrce dds flow control flag (#26209 ) * added flow control bitfield * shortened to 16 char param name length * refactored changes for uxrce flow control param * reverted additions to docs * Update src/modules/uxrce_dds_client/module.yaml Co-authored-by: Hamish Willee <hamishwillee@gmail.com> --------- Co-authored-by: minotico <81227020-minotico@users.noreply.github.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Co-authored-by: Hamish Willee <hamishwillee@gmail.com>	2026-01-07 16:59:03 -09:00
Erkki Eilonen	598066653a	cmake: set CMAKE_CXX_STANDARD 17 to match code assumptions (#25767 ) Usage of std::in_place_t implies C++17	2026-01-07 16:56:16 -09:00
Claudio Chies	86e1356e0a	Extend SENS_GPS_PRIME usage for UAVCAN GNSS devices (#26126 ) * UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices * use convenience function Co-authored-by: Matthias Grob <maetugr@gmail.com> * Update src/drivers/uavcan/sensors/gnss.cpp Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> * Apply suggestion from @MaEtUgR Co-authored-by: Matthias Grob <maetugr@gmail.com> * Fix type casting in GPS prime range check * reverted parameter default * UAVCAN: fix and improve device_id logic (#26135) * UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices * use convenience function Co-authored-by: Matthias Grob <maetugr@gmail.com> * Update src/drivers/uavcan/sensors/gnss.cpp Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> * Apply suggestion from @MaEtUgR Co-authored-by: Matthias Grob <maetugr@gmail.com> * Fix type casting in GPS prime range check * UAVCAN: fix and improve device_id logic * Added bus information to more UAVCAN drivers * Fix device_id registration in UavcanBarometerBridge --------- Co-authored-by: Matthias Grob <maetugr@gmail.com> Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --------- Co-authored-by: Matthias Grob <maetugr@gmail.com> Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>	2026-01-07 10:19:07 -09:00
Balduin	450cf79fc8	FwLateralLongitudinalControl: Publish flight phase (#26219 ) * FwLateralLongitudinalControl: publish flight phase * FwLateralLongitudinalControl: consolidate hrt_absolute_time calls * FwLateralLongitudinalControl: Name time variables correctly * FwLateralLongitudinalControl: pass current time as argument rather than class member * FwLateralLongitudinalControl: use local position timestamp	2026-01-07 11:10:38 +01:00
Claudio Chies	1ff36422c9	docs: UAVCAN asset tracking (#26152 )	2026-01-07 14:46:15 +11:00
Nick	38b6a9abf3	pwm: Add PWM center support to Wheel and Gimbal (#26211 ) * Add Wheel and Gimbal support to PWM center * Document Center feature for PWM Gimbal	2026-01-06 18:05:35 +01:00