/*=====================================================================================
 File name:        PID_REG3.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:  The PID controller with anti-windup                   

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/

#include "pid_reg3.h"

#include "IQmathLib.h"         // Include header for IQmath library

#define IQ_100 1677721600

#pragma CODE_SECTION(pid_reg3_calc,".fast_run");
void pid_reg3_calc(PIDREG3 *v)
{
_iq Ud2;
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    // Compute the proportional output
    v->Up = _IQmpy(v->Kp,v->Err);

    // Compute the integral output
    v->Ui = v->Ui + _IQmpy(v->Ki,v->Up) + _IQmpy(v->Kc,v->SatErr);
/*
    // Saturate the integral output
    if (v->Ui > v->OutMax)
      v->Ui =  v->OutMax;
    else if (v->Ui < v->OutMin)
      v->Ui =  v->OutMin;
*/
    // Compute the derivative output
	Ud2 = v->Up - v->Up1;// _IQmpy(IQ_100,(v->Up - v->Up1));
    v->Ud = _IQmpy(v->Kd,Ud2);


    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
      v->Out =  v->OutMax;
    else if (v->OutPreSat < v->OutMin)
      v->Out =  v->OutMin;
    else
      v->Out = v->OutPreSat;

    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;

    // Update the previous proportional output
    v->Up1 = v->Up;


}