/*
 * alg_uf_const.c
 *
 *  Created on: 26 θών. 2020 γ.
 *      Author: Yura
 */
#include <alg_uf_const.h>
#include <edrk_main.h>
#include <params_alg.h>
#include <params_norma.h>
#include <params_pwm24.h>

#include "mathlib.h"




#define T_NARAST    15 // ρεκ.

//VHZPROF vhz1 =  VHZPROF_DEFAULTS;


ALG_UF_CONST uf_const1 = ALG_UF_CONST_DEFAULT;

void init_uf_const(void)
{
    uf_const1.freq = 0;
    uf_const1.km = 0;

    uf_const1.zad_plus_km = _IQ(1.0/(FREQ_PWM*T_NARAST));


    uf_const1.rmp_freq.RampLowLimit = _IQ(-4); //0
    uf_const1.rmp_freq.RampHighLimit = _IQ(4);

    uf_const1.rmp_freq.RampPlus = _IQ(0.0002);

    uf_const1.rmp_freq.RampMinus = _IQ(-0.0002);
    uf_const1.rmp_freq.DesiredInput = 0;
    uf_const1.rmp_freq.Out = 0;

    uf_const1.max_km =   _IQ(K_STATOR_MAX);

}


void uf_const(_iq *Fz, _iq *Uz1, _iq *Uz2)
{


//    vhz1.HighFreq = _IQ(f.fmax_uf/F_STATOR_MAX);
/////////////

    uf_const1.km = edrk.zadanie.iq_kzad_rmp;
//    uf_const1.km = zad_intensiv_q(uf_const1.zad_plus_km, uf_const1.zad_plus_km, uf_const1.km, edrk.zadanie.iq_kzad);
//    uf_const1.km = _IQsat(uf_const1.km,uf_const1.max_km,0);
    *Uz1 = uf_const1.km;
    *Uz2 = uf_const1.km;


/////////////////
    uf_const1.freq = edrk.zadanie.iq_fzad_rmp;

//    uf_const1.rmp_freq.DesiredInput = uf_const1.freq;
//    uf_const1.rmp_freq.calc(&uf_const1.rmp_freq);
    *Fz = uf_const1.freq;

/*
    vhz1.Freq = Fzad;
    vhz1.calc(&vhz1);



    *Fz = rmp_freq.Out;
 */


}