Tecs: restore pitch integrator when fast descends ends

[KonradRudin]

Solved Problem

When engaging fast descend the pitch integrator usually changes to pitch, as the pitch control loop controls the airspeed. When the fast descend ends and the AV approaches the target altitude, the Av thus tends to undershoot it a bit until the pitch integrator is back close to the original value.

Fixes #{Github issue ID}

Solution

• Save the pitch integrator value, when first entering the fast descend mode and restore its value when leaving fast descend
• Make sure that the transition is smooth by interpolating the integrator value when switching

Changelog Entry

For release notes:

Bugfix: Avoid undershoot when leaving the fast descend mode by resetting the pitch integrator value

Test coverage

Flight log of integrator without the fix:

Flight log with integrator fix:

Still needs to be tested!

[github-actions]

🔎 FLASH Analysis

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +192  +0.0%    +192    .text
  +4.2%    +184  +4.2%    +184    ../../src/lib/tecs/TECS.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +201  [ = ]       0    .debug_info
  +0.5%    +191  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +14  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%    +279  [ = ]       0    .debug_line
  +3.7%    +313  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -12  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.3%      +3  [ = ]       0    task/task_cancelpt.c
+0.0%    +269  [ = ]       0    .debug_loc
  +2.1%    +269  [ = ]       0    ../../src/lib/tecs/TECS.cpp
+0.0%     +40  [ = ]       0    .debug_ranges
  +3.0%     +64  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%     +23  [ = ]       0    .debug_str
  +0.0%     +23  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%     +16  [ = ]       0    .symtab
  +1.4%     +16  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-2.0%    -192  [ = ]       0    [Unmapped]
+0.0%    +880  +0.0%    +192    TOTAL

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +192  +0.0%    +192    .text
  +4.2%    +184  +4.2%    +184    ../../src/lib/tecs/TECS.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +201  [ = ]       0    .debug_info
  +0.5%    +191  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +14  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%    +279  [ = ]       0    .debug_line
  +3.7%    +313  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -12  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.3%      +3  [ = ]       0    task/task_cancelpt.c
+0.0%    +254  [ = ]       0    .debug_loc
  +2.1%    +269  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -0.0%     -15  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%     +39  [ = ]       0    .debug_ranges
  +3.0%     +64  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.5%      -1  [ = ]       0    task/task_cancelpt.c
+0.0%     +23  [ = ]       0    .debug_str
  +0.0%     +23  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
+0.0%     +16  [ = ]       0    .symtab
  +1.4%     +16  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-0.3%    -192  [ = ]       0    [Unmapped]
+0.0%    +864  +0.0%    +192    TOTAL

Updated: 2025-01-15T10:04:48

[bresch]

Isn't "fast descend" some emergency logic that should only kick in some rare cases when altitude tracking is degraded (e.g.: wind gust)? In an emergency case that should never occur, do we really need a perfect behavior?

To explain a bit my question, my fear is that we start to completely rely on fast descend because tuning a vehicle is too complicated and that a ton of extra logic needs to be added as an attempt to shape the desired response. The result would be an overcompicated TECS code with a completely nonlinear an unpredictable behavior.

[KonradRudin]

Isn't "fast descend" some emergency logic that should only kick in some rare cases when altitude tracking is degraded (e.g.: wind gust)? In an emergency case that should never occur, do we really need a perfect behavior?

No, when fast descend is enabled it will always be on when trying to descend above a threshold. We use it to cut down the loiter down time significantly. The problem with the other TECS logic is that it will always try to keep the cruising airspeed as well so the descend speed is limited by that. In fast descend we allow for descending with higher airspeed, something you nromally want to have as an option. So not only emergencies

To explain a bit my question, my fear is that we start to completely rely on fast descend because tuning a vehicle is too complicated and that a ton of extra logic needs to be added as an attempt to shape the desired response. The result would be an overcompicated TECS code with a completely nonlinear an unpredictable behavior.

I get what you mean. Ideally there is an easier way to switch TECS between at least a few modes (Normal altitude/airspeed tracking, thrust/speed tracking where i can define a thrust input and define speed to be tracked for pitch loop, and maybe altitude tracking only). But that would mean to make Tecs structured a bit better and have those modes separated

[KonradRudin]

Maybe we can even et away with it by just tuning for faster ptch integrator and changing the speed weight.
Also i need to check if the feedforward term is correct for the fast descend mode.