fix(GotoControl): handle EKF resets correctly

[MaEtUgR]

Solved Problem

1. Fix goto setpoint yaws smoother using heading rate and acceleration defined in °/s(²) as rad/s(²).
2. Fix goto setpoint not handling EKF resets correctly.

Solution

1. Rename the internal variable to make clear it's a separate SI units radians state and have the correct conversion for the parameter in the setter function.
2. Handle EKF resets as picked up by the reset counter check in MulticopterPositionControl.cpp and correctly reset/update internal states of the position and heading smoothing. This results in smoothly going back to the original goto setpoint if it stays unchanged compared to jumping back to it with an unexpected transient.

Changelog Entry

fix(GotoControl): handle EKF resets correctly

Test coverage

I went through simulating with make px4_sitl sihsim_hex, commanding a stationary goto setpoint and causing an EKF reset by injecting an erroneuous estimate with the vehicle commands MAV_CMD_EXTERNAL_POSITION_ESTIMATE and MAV_CMD_EXTERNAL_ATTITUDE_ESTIMATE.

Position reset

before:

after:

Heading reset

before:

with reset handling but still wrong units:

after:

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 128 byte (0.01 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +128  +0.0%    +128    .text
     +29%    +116   +29%    +116    MulticopterPositionControl::adjustSetpointForEKFResets()
    +0.6%     +20  +0.6%     +20    MulticopterPositionControl::parameters_update()
     +14%      +2   +14%      +2    AlphaFilter<>::AlphaFilter()
    +1.0%      +2  +1.0%      +2    MulticopterPositionControl::~MulticopterPositionControl()
    -8.3%      -4  -8.3%      -4    Navigator::stop_capturing_images()
    -7.4%      -8  -7.4%      -8    FlightTaskAuto::_ekfResetHandlerHeading()
  +0.0%     +53  [ = ]       0    .debug_abbrev
  +0.0%     +16  [ = ]       0    .debug_aranges
  +0.0%     +40  [ = ]       0    .debug_frame
  +0.0% +2.41Ki  [ = ]       0    .debug_info
  +0.0%    +947  [ = ]       0    .debug_line
    [NEW]      +7  [ = ]       0    [Unmapped]
    +0.0%    +940  [ = ]       0    [section .debug_line]
  +0.0%   +1024  [ = ]       0    .debug_loclists
    [NEW]      +3  [ = ]       0    [Unmapped]
    +0.0%   +1021  [ = ]       0    [section .debug_loclists]
  +0.0%    +225  [ = ]       0    .debug_rnglists
  +0.0%    +247  [ = ]       0    .debug_str
  -1.1%    -128  [ = ]       0    [Unmapped]
  +0.0% +4.90Ki  +0.0%    +128    TOTAL

px4_fmu-v6x [Total VM Diff: 128 byte (0.01 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +128  +0.0%    +128    .text
     +29%    +116   +29%    +116    MulticopterPositionControl::adjustSetpointForEKFResets()
    +0.6%     +20  +0.6%     +20    MulticopterPositionControl::parameters_update()
     +14%      +2   +14%      +2    AlphaFilter<>::AlphaFilter()
    +1.0%      +2  +1.0%      +2    MulticopterPositionControl::~MulticopterPositionControl()
    -8.3%      -4  -8.3%      -4    Navigator::stop_capturing_images()
    -7.4%      -8  -7.4%      -8    FlightTaskAuto::_ekfResetHandlerHeading()
  +0.0%     +53  [ = ]       0    .debug_abbrev
  +0.0%     +16  [ = ]       0    .debug_aranges
  +0.0%     +40  [ = ]       0    .debug_frame
  +0.0% +2.41Ki  [ = ]       0    .debug_info
  +0.0%    +939  [ = ]       0    .debug_line
   -20.0%      -1  [ = ]       0    [Unmapped]
    +0.0%    +940  [ = ]       0    [section .debug_line]
  +0.0%   +1020  [ = ]       0    .debug_loclists
   -33.3%      -1  [ = ]       0    [Unmapped]
    +0.0%   +1021  [ = ]       0    [section .debug_loclists]
  +0.0%    +225  [ = ]       0    .debug_rnglists
  +0.0%    +247  [ = ]       0    .debug_str
  -1.9%    -128  [ = ]       0    [Unmapped]
  +0.0% +4.89Ki  +0.0%    +128    TOTAL

Updated: 2026-09-22T09:40:45