feat(FWModeManager): runway takeoff set yaw setpoint to bearing to takeoff waypoint

[sfuhrer]

Solved Problem

In older PX4 versions we used to steer the vehicle towards the takeoff waypoint with the wheel controller. This got removed in #24056. Since then the wheel controller just tracks the heading it was initialized with. When the vehicle is well aligned with runway center line it may get off the runway during the takeoff.

Solution

Set the wheel controller heading setpoint to the bearing from "takeoff start" point (measurement) to "takeoff waypoint" (planned).

Changelog Entry

For release notes:

Feature: Runway takeoff set yaw setpoint to bearing to takeoff waypoint

Alternatives

• We used to have a param (RWTO_HDG) to chose between 0 = hold initial airframe heading, 1 = track heading towards the takeoff waypoint. I would prefer to avoid adding back a setting that few people really ever cared about.
• Instead of doing simple bearing tracking, we could do "line tracking". The controller would then use DirectionalGuidance::guideToPath()

Test coverage

Untested.

Context

Related links, screenshot before/after, video

[sfuhrer]

@ryanjAA what's your opinion here? Bring back RWTO_HDG or agree on either option.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 48 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +48  +0.0%     +48    .text
    +1.5%     +24  +1.5%     +24    FixedwingAttitudeControl::Run()
    +0.1%     +12  +0.1%     +12    uORB::compressed_fields
    +0.6%      +8  +0.6%      +8    FixedWingModeManager::control_auto_landing_circular()
    +0.6%      +8  +0.6%      +8    FixedWingModeManager::control_auto_landing_straight()
    +0.3%      +4  +0.3%      +4    FixedWingModeManager::control_auto_takeoff()
    +0.5%      +4  +0.5%      +4    FixedWingModeManager::vehicle_attitude_poll()
   -33.3%      -4 -33.3%      -4    Zero
    -0.6%      -4  -0.6%      -4    land_detector::FixedwingLandDetector::_get_landed_state()
   -20.0%      -4 -20.0%      -4    logger_main
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +66  [ = ]       0    .debug_info
  +0.0%     +23  [ = ]       0    .debug_line
   -85.7%      -6  [ = ]       0    [Unmapped]
    +0.0%     +29  [ = ]       0    [section .debug_line]
  +0.0%     +69  [ = ]       0    .debug_loclists
  +0.0%      +7  [ = ]       0    .debug_rnglists
    [NEW]      +2  [ = ]       0    [Unmapped]
    +0.0%      +5  [ = ]       0    [section .debug_rnglists]
  +0.0%     +28  [ = ]       0    .debug_str
  -0.6%     -48  [ = ]       0    [Unmapped]
  +0.0%    +248  +0.0%     +48    TOTAL

px4_fmu-v6x [Total VM Diff: 56 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +56  +0.0%     +56    .text
    +1.5%     +24  +1.5%     +24    FixedwingAttitudeControl::Run()
    +0.1%     +12  +0.1%     +12    uORB::compressed_fields
    +0.6%      +8  +0.6%      +8    FixedWingModeManager::control_auto_landing_circular()
    +0.6%      +8  +0.6%      +8    FixedWingModeManager::control_auto_landing_straight()
    +0.3%      +4  +0.3%      +4    FixedWingModeManager::control_auto_takeoff()
    +0.5%      +4  +0.5%      +4    FixedWingModeManager::vehicle_attitude_poll()
     +50%      +4   +50%      +4    Zero
    -0.6%      -4  -0.6%      -4    land_detector::FixedwingLandDetector::_get_landed_state()
   -20.0%      -4 -20.0%      -4    logger_main
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +66  [ = ]       0    .debug_info
  +0.0%     +31  [ = ]       0    .debug_line
     +67%      +2  [ = ]       0    [Unmapped]
    +0.0%     +29  [ = ]       0    [section .debug_line]
  +0.0%     +79  [ = ]       0    .debug_loclists
  +0.0%      +5  [ = ]       0    .debug_rnglists
  +0.0%     +28  [ = ]       0    .debug_str
  -1.0%     -56  [ = ]       0    [Unmapped]
  +0.0%    +264  +0.0%     +56    TOTAL

Updated: 2026-07-10T09:32:55

[ryanjAA]

@sfuhrer I’m fine not bringing RWTO_HDG back, but I don’t think we should merge the constant-bearing version. Feeding the wheel controller sp.course_setpoint (which is already returned by navigateLine rather than the raw takeoff_bearing)

Before #24056 , runway takeoff ran NPFG line guidance and passed _npfg.getBearing() into the yaw/wheel controller (sp.course_setpoint is the current equivalent I think).

The raw takeoff_bearing only commands the tangent of the takeoff line. It will correct an initial heading error, but after a lateral disturbance, wheel bias (which we often see), GNSS shift, wind, it has no mechanism to recapture the line. the aircraft can continue parallel to it. On our large planes with a long ground roll, that matters. A good thing we always tell people is to think about how much a 3° error over 200m is… i.e. ~10.5m laterally..

Another point, #24056 also removed RWTO_NPFG_PERIOD, so using sp.course_setpoint restores the old command structure but not necessarily the exact old ground-roll tuning. I don’t think that means we need the RWTO_HDG toggle back, but we should verify that the normal NPFG period gives acceptable wheel-steering response or decide whether a ground-specific period is still needed.

My vote: hardcode line tracking on, with no RWTO_HDG parameter:

fw_runway_control.wheel_steering_yaw_setpoint = sp.course_setpoint;

Before merging, can we run SITL cases with an initial heading error, a lateral displacement or wheel bias introduced during the roll, and crosswind? I’d specifically want yo ensure cross-track convergence, wheel saturation, and whether the corrected-course command introduces any oscillation.

This is also the right primitive for future taxi/ground-steering waypoint support (which we will push soon from prev version). The mode manager generates a path-corrected course and the wheel controller tracks it.

[mahima-yoga]

I agree with @ryanjAA - but this PR is already a strict improvement compared to the current behavior isn't it?

For larger vehicles with longer runway time it's probably not ideal, but for smaller ones it's probably already sufficient. If there's time to implement the line tracking though then it would probably be the best solution

[sfuhrer]

I went back and forth between line tracking and bearing tracking. In the end I preferred bearing for the sake of simplicity. The line tracking brings NPFG back into the control loop, which is optimized for the flying state and not separately tunable.
@ryanjAA could you actually support on the testing front? I don't think SITL testing will bring us far, the vehicle-to-ground interaction physics are rather poorly modeled there afaik.