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