TECS to keep demanded airspeed in a custom mode

Hello. I am developing a custom mode for ArduPlane where I control roll, pitch and yaw following custom experimental guidance laws. While in this mode I want to keep constant air speed regardless of maneuvers(as much as possible ofc).

Looking into TECS code I see it takes speed demand as well as height demand simultaneously into account. Is there a way to disregard height and make it effectively work as “increase throttle if speed is too low, decrease otherwise” control loop? I want it to control exclusively speed by adjusting throttle.

For now I put lines like below:

 plane.set_target_altitude_current();
  plane.calc_throttle();

into custom mode code but in SITL it behaves strange, when pitch is negative and speed exceeds demand it keeps increasing throttle for some reason.

Any suggestions or examples how to do it correctly? Thanks in advance.

You can try taking a look at plane acrobatics Lua.

TECS is energy controller, it has to control both altitude and speed by definition. If you are already implementing YPR control it shouldn’t be hard to implement speed PID that should work decently well.