Remove multicopter autonomous flight offtrack logic

[MaEtUgR]

Solved Problem

Since the beginning of FlightTaskAuto covering the navigator triplet interface for most autonomous multicopter flight there's the "offtrack" logic which made sure nothing too bad happens if the vehicle is too far away from the line between two waypoints or overshoots the goal waypoint.

See e.g. 6e62beb

This logic duplicates much of the position/velocity smoothing trajectory planner, which is a coarse path following logic. Now the offtrack logic can lead to unexpected behavior and regularly problems which are hard to debug. Last issue with hacky workaround for reference: #25725

Solution

Remove the offtrack logic and test that the corner cases still work fine because the trajectory generation does better path tracking than the offtrack logic.

Changelog Entry

Remove multicopter autonomous flight offtrack logic

Test coverage

Testing the corner cases in SITL SIH:

• Bad velocity tracking leading to overshoots and not following the line exactly.

The vehicle slows down when it gets far from the line but no sudden jerky unexpected movements anymore.

• Stopping the mission between two waypoints, flying away in position and resuming the mission

The vehicle does not come back to the line but rather directly heads to the next waypoint. Remains to be fixed.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -656 byte (-0.03 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +49  [ = ]       0    .debug_abbrev
-0.0%     -24  [ = ]       0    .debug_aranges
-0.0%     -64  [ = ]       0    .debug_frame
-0.0% -6.25Ki  [ = ]       0    .debug_info
-0.0% -1.89Ki  [ = ]       0    .debug_line
 -40.0%      -2  [ = ]       0    [Unmapped]
  -0.0% -1.89Ki  [ = ]       0    [section .debug_line]
-0.1% -5.11Ki  [ = ]       0    .debug_loclists
-0.1%    -326  [ = ]       0    .debug_rnglists
-0.0%    -143  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
-0.0%    -146  [ = ]       0    .strtab
  [NEW]     +56  [ = ]       0    FlightTaskAuto::_evaluatePositionSetpointTriplet()
  [DEL]     -41  [ = ]       0    FlightTaskAuto::_evaluateTriplets()
  [DEL]     -40  [ = ]       0    FlightTaskAuto::_getCurrentState()
  [DEL]     -48  [ = ]       0    FlightTaskAuto::_updateInternalWaypoints()
  [DEL]     -73  [ = ]       0    PositionSmoothing::_getCrossingPoint()
   +64%     +16  [ = ]       0    __nxsem_clockwait_veneer
 -50.0%     -16  [ = ]       0    __strcmp_veneer
-0.0%    -160  [ = ]       0    .symtab
  [NEW]     +96  [ = ]       0    FlightTaskAuto::_evaluatePositionSetpointTriplet()
  [DEL]     -96  [ = ]       0    FlightTaskAuto::_evaluateTriplets()
  [DEL]     -48  [ = ]       0    FlightTaskAuto::_getCurrentState()
  [DEL]     -64  [ = ]       0    FlightTaskAuto::_updateInternalWaypoints()
 -16.7%     -16  [ = ]       0    FlightTaskAuto::update()
  [DEL]     -16  [ = ]       0    PositionSmoothing::_getCrossingPoint()
 -25.0%     -16  [ = ]       0    PositionSmoothing::_getL1Point()
  -0.3%     -32  [ = ]       0    [section .symtab]
   +67%     +32  [ = ]       0    __nxsem_clockwait_veneer
 -40.0%     -32  [ = ]       0    __strcmp_veneer
  +1.9%     +16  [ = ]       0    px4::wq_configurations::nav_and_controllers
   +50%     +16  [ = ]       0    sensors::VehicleGPSPosition::PrintStatus()
+7.3%    +656  [ = ]       0    [Unmapped]
-0.0%    -656  -0.0%    -656    .text
  [NEW]    +944  [NEW]    +944    FlightTaskAuto::_evaluatePositionSetpointTriplet()
   +16%     +96   +16%     +96    Mission::setActiveMissionItems()
   +18%     +48   +18%     +48    PositionSmoothing::_getL1Point()
   +19%     +28   +19%     +28    matrix::Vector<>::unit_or_zero()
   +12%      +8   +12%      +8    FlightTaskAuto::_ekfResetHandlerHeading()
  +1.4%      +8  +1.4%      +8    FlightTaskAuto::update()
  +0.0%      +8  +0.0%      +8    [section .text]
  +4.0%      +4  +4.0%      +4    FlightModeManager::tryApplyCommandIfAny()
  +1.9%      +4  +1.9%      +4    FlightTaskAuto::_checkEmergencyBraking()
  +0.5%      +4  +0.5%      +4    FlightTaskAuto::_prepareLandSetpoints()
  +1.6%      +4  +1.6%      +4    FlightTaskAuto::_set_heading_from_mode()
 -99.6%      +4 -99.6%      +4    [5 Others]
  -1.1%      -4  -1.1%      -4    FlightModeManager::Run()
  -1.3%      -8  -1.3%      -8    FlightModeManager::FlightModeManager()
  -1.1%      -8  -1.1%      -8    PositionSmoothing::_generateVelocitySetpoint()
  -4.9%     -20  -4.9%     -20    FlightTaskAuto::_updateTrajConstraints()
  [DEL]     -56  [DEL]     -56    PositionSmoothing::_getCrossingPoint()
  -8.8%     -84  -8.8%     -84    FlightTaskAuto::FlightTaskAuto()
  [DEL]    -132  [DEL]    -132    FlightTaskAuto::_updateInternalWaypoints()
  [DEL]    -492  [DEL]    -492    FlightTaskAuto::_getCurrentState()
  [DEL]   -1012  [DEL]   -1012    FlightTaskAuto::_evaluateTriplets()
-0.0% -14.1Ki  -0.0%    -656    TOTAL

px4_fmu-v6x [Total VM Diff: -656 byte (-0.03 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +49  [ = ]       0    .debug_abbrev
-0.0%     -24  [ = ]       0    .debug_aranges
-0.0%     -64  [ = ]       0    .debug_frame
-0.0% -6.25Ki  [ = ]       0    .debug_info
-0.0% -1.89Ki  [ = ]       0    .debug_line
 -66.7%      -2  [ = ]       0    [Unmapped]
  -0.0% -1.89Ki  [ = ]       0    [section .debug_line]
-0.1% -5.11Ki  [ = ]       0    .debug_loclists
-0.1%    -326  [ = ]       0    .debug_rnglists
-0.0%    -143  [ = ]       0    .debug_str
+0.9%      +2  [ = ]       0    .shstrtab
-0.0%    -146  [ = ]       0    .strtab
  [NEW]     +56  [ = ]       0    FlightTaskAuto::_evaluatePositionSetpointTriplet()
  [DEL]     -41  [ = ]       0    FlightTaskAuto::_evaluateTriplets()
  [DEL]     -40  [ = ]       0    FlightTaskAuto::_getCurrentState()
  [DEL]     -48  [ = ]       0    FlightTaskAuto::_updateInternalWaypoints()
  [DEL]     -73  [ = ]       0    PositionSmoothing::_getCrossingPoint()
-0.0%    -160  [ = ]       0    .symtab
  [NEW]     +96  [ = ]       0    FlightTaskAuto::_evaluatePositionSetpointTriplet()
  [DEL]     -96  [ = ]       0    FlightTaskAuto::_evaluateTriplets()
  [DEL]     -48  [ = ]       0    FlightTaskAuto::_getCurrentState()
  [DEL]     -64  [ = ]       0    FlightTaskAuto::_updateInternalWaypoints()
 -16.7%     -16  [ = ]       0    FlightTaskAuto::update()
  [DEL]     -16  [ = ]       0    PositionSmoothing::_getCrossingPoint()
 -25.0%     -16  [ = ]       0    PositionSmoothing::_getL1Point()
  -0.3%     -32  [ = ]       0    [section .symtab]
  +1.9%     +16  [ = ]       0    px4::wq_configurations::nav_and_controllers
   +50%     +16  [ = ]       0    sensors::VehicleGPSPosition::PrintStatus()
+9.9%    +656  [ = ]       0    [Unmapped]
-0.0%    -656  -0.0%    -656    .text
  [NEW]    +944  [NEW]    +944    FlightTaskAuto::_evaluatePositionSetpointTriplet()
   +16%     +96   +16%     +96    Mission::setActiveMissionItems()
   +18%     +48   +18%     +48    PositionSmoothing::_getL1Point()
   +19%     +28   +19%     +28    matrix::Vector<>::unit_or_zero()
   +12%      +8   +12%      +8    FlightTaskAuto::_ekfResetHandlerHeading()
  +1.4%      +8  +1.4%      +8    FlightTaskAuto::update()
  +0.0%      +8  +0.0%      +8    [section .text]
  +4.0%      +4  +4.0%      +4    FlightModeManager::tryApplyCommandIfAny()
  +1.9%      +4  +1.9%      +4    FlightTaskAuto::_checkEmergencyBraking()
  +0.5%      +4  +0.5%      +4    FlightTaskAuto::_prepareLandSetpoints()
  +1.6%      +4  +1.6%      +4    FlightTaskAuto::_set_heading_from_mode()
 -99.6%      +4 -99.6%      +4    [5 Others]
  -1.1%      -4  -1.1%      -4    FlightModeManager::Run()
  -1.3%      -8  -1.3%      -8    FlightModeManager::FlightModeManager()
  -1.1%      -8  -1.1%      -8    PositionSmoothing::_generateVelocitySetpoint()
  -4.9%     -20  -4.9%     -20    FlightTaskAuto::_updateTrajConstraints()
  [DEL]     -56  [DEL]     -56    PositionSmoothing::_getCrossingPoint()
  -8.8%     -84  -8.8%     -84    FlightTaskAuto::FlightTaskAuto()
  [DEL]    -132  [DEL]    -132    FlightTaskAuto::_updateInternalWaypoints()
  [DEL]    -492  [DEL]    -492    FlightTaskAuto::_getCurrentState()
  [DEL]   -1012  [DEL]   -1012    FlightTaskAuto::_evaluateTriplets()
-0.0% -14.0Ki  -0.0%    -656    TOTAL

Updated: 2026-01-30T14:22:56

[dakejahl]

I am in favor of this!

[mahima-yoga]

If I understand it correctly: the "previous setpoint" was stored in one of these intermediate states in the off-track logic. So by removing it, knowledge of the previous setpoint was also removed. Rather than always tracking an extra state, I thought it maybe makes more sense to actually populate the position_setpoint_triplet.previous in the mission.cp when re-activating it. I couldn't see a direct drawback to do this, but happy to have some input on whether this is not a clean solution 😃.

I have re-added the logic to return to the mission line, maintaining the behaviour which was implemented in off-track initially. Tested in SITL (sihsim), with the mission below.

1. Pilot takes over between WP3 and WP4 and moves behind WP3. Upon mission re-activation: vehicle goes to WP3 and proceeds to WP4.

2. Pilot takes over between WP3 and WP4 and moves ahead of WP4. Upon mission re-activation: vehicle goes to WP4 and proceeds to WP5.

3. Pilot takes over between WP4 and WP5 and moves "off-track" (some point between the two). Upon mission re-activation: vehicle flies back to mission line and proceeds to WP5.

[MaEtUgR]

Thanks a lot @mahima-yoga !! 🎉

I thought it maybe makes more sense to actually populate the position_setpoint_triplet.previous in the mission.cp when re-activating it.
happy to have some input on whether this is not a clean solution 😃

That's exactly what I thought. As long as Navigator provides the waypoints and we can't request them (more simple mission interface) it should populate the previous waypoint when continuing the mission. But I wasn't sure how easy that is. Your described examples look perfect. Things I'd like to find out in review:

• How did the pr end up adding 3 lines in total even though it was aiming at removing logic to makes things simpler? Did I remove something useful then or is there a better implementation now? Is it worth simplifying still?
• Does it potentially break e.g. fixed wing mission behavior? Sadly currently always a risk when changing global logic that tries to accommodate all vehicle types.

[MaEtUgR]

the "previous setpoint" was stored in one of these intermediate states in the off-track logic

It cannot be stored in FlightTaskAuto while flying non-autonomous modes like e.g. Position. The entire flight task class is destroyed and rebuilt on mode changes, no accidental old state kept. I didn't check where the information for the previous point for the line reapproach came from but based on the above it must be available.

[nlsxp]

I have two questions to this after looking into it for this issue:
Why was the Commit to rejoin ultimately reversed?
Why do we not simply continue to the next waypoint from wherever we are at that moment?

From my side looking into obstacle avoidance, I find it undesirable to go directly back to the closest point, like shown in the comment from @mahima-yoga.

[mahima-yoga]

Why was the Commit to rejoin ultimately reversed?

It was reverted because this commit introduces a much smaller change that achieves the same result