// #include "DSP281x_Device.h"     // DSP281x Headerfile Include File
// #include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "IQmathLib.h"
#include "my_filter.h"


// #pragma CODE_SECTION(exp_regul_iq19,".fast_run");
_iq19 exp_regul_iq19(_iq19 k_exp_regul, _iq19 InpVarCurr, _iq19 InpVarInstant)
{
 _iq19 t1;

 t1 = (InpVarCurr + _IQ19mpy( (InpVarInstant-InpVarCurr),  k_exp_regul));
 return t1;
}


// #pragma CODE_SECTION(exp_regul_iq,".fast_run");
_iq exp_regul_iq(_iq k_exp_regul, _iq InpVarCurr, _iq InpVarInstant)
{
 _iq t1;
 t1 = (InpVarCurr + _IQmpy( (InpVarInstant-InpVarCurr),  k_exp_regul));
 return t1;
}

// #pragma CODE_SECTION(exp_regul_iq,".fast_run");
void exp_regul_iq_fast(_iq k_exp_regul, _iq *InpVarCurr, _iq InpVarInstant)
{
 _iq t1;
 volatile _iq t2,t3,t4;

  
  t2 = (InpVarInstant- *InpVarCurr);
  t3 =  _IQmpy( t2,  k_exp_regul);
  t4 = *InpVarCurr + t3;
  t1 = t4;
  *InpVarCurr = t1;
// t1 = (InpVarCurr + _IQmpy( (InpVarInstant-InpVarCurr),  k_exp_regul));
// return t1;
}

_iq zad_intensiv_q(_iq StepP, _iq StepN, _iq InpVarCurr, _iq InpVarInstant)
{ 
    _iq deltaVar, VarOut;
    
    deltaVar = InpVarInstant-InpVarCurr;
    
    if ((deltaVar>StepP) || (deltaVar<-StepN))
    {
      if (deltaVar>0) InpVarCurr += StepP;
      else InpVarCurr -= StepN;
    }
    else 
      InpVarCurr=InpVarInstant;

    
    VarOut = InpVarCurr;
    return VarOut;       


}





/* 

_iq18 filter_1p(_iq18 predx,_iq18 predy, _iq18 inpx)
{
  _iq18 t1;

  t1 = _IQ18mpy(k1_filter_1p_fast,(predx+inpx))+_IQ18mpy(k2_filter_1p_fast,predy);
  return t1;
}


*/

/*

void init_filter_batter2()
{
  u_filter_batter2[0]=0;
  u_filter_batter2[1]=0;
  u_filter_batter2[2]=0;

  i_filter_batter2[0]=0;
  i_filter_batter2[1]=0;
  i_filter_batter2[2]=0;

  k_filter_batter2[0]=_IQ(K1_FILTER_BATTER2_3HZ);
  k_filter_batter2[1]=_IQ(K2_FILTER_BATTER2_3HZ);
  k_filter_batter2[2]=_IQ(K3_FILTER_BATTER2_3HZ  );

}




//#pragma CODE_SECTION(filter_batter2,".fast_run");
_iq filter_batter2(_iq InpVarCurr)
{
_iq y;
   
    y = _IQmpy(k_filter_batter2[0],( InpVarCurr + _IQmpyI32(i_filter_batter2[0],2) + i_filter_batter2[1]   )  ) + 
               _IQmpy(k_filter_batter2[1], u_filter_batter2[0]) + _IQmpy(k_filter_batter2[2], u_filter_batter2[1]);

    u_filter_batter2[1]=u_filter_batter2[0];
	u_filter_batter2[0]=y;

	i_filter_batter2[1]=i_filter_batter2[0];
	i_filter_batter2[0]=InpVarCurr;
    return y;

}


*/




