PID Implementation

The PID controller use proportional, integral, derivative and output derivative gain with fixed point arithmetic.

Proportional = Proportional_gain * (Consigne - Reference)

if Integral_gain == 0
    Integral = 0
else
    Integral = Integral_gain * (Consigne - Reference) + Previous_integral_gain_sum
    
We saturate the integral term with uIAntiWU_Limit

if Integral > uIAntiWU_Limit
    Integral = uIAntiWU_Limit
if Integral < -uIAntiWU_Limit
    Integral = -uIAntiWU_Limit

Derivative = Derivative_gain * (Error - Previous_error) with Error = (Consigne - Reference)

Output_Derivative = Output_derivative_gain * (PID_Output - Previous_PID_output)

(Proportional + Integral + Derivative + Output_Derivative) / Fixed_point_divider