====== PID Implementation ======

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

----

===== Proportional =====

<code>
Proportional = Proportional_gain * (Consigne - Reference)
</code>
----


===== Integral =====

<code>
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
</code>
----

===== Derivative =====

<code>
Derivative = Derivative_gain * (Error - Previous_error) with Error = (Consigne - Reference)
</code>
----


===== Output Derivative =====

<code>
Output_Derivative = Output_derivative_gain * (PID_Output - Previous_PID_output)
</code>
----


===== PID Output =====

<code>
(Proportional + Integral + Derivative + Output_Derivative) / Fixed_point_divider
</code>
----