Constrain the specific energy balance integrator input to prevent increasing saturation of pitch demand.
Decay the specific energy balance integrator state if the pitch demand is saturated to reduce saturation to zero and do so at the same tome constant as the control loop
Relax the clipping threshold on the specific energy balance integrator to allow the input constraint and decay functions to do more of the work
Improve variable naming and commenting