/**************************************************************************
	Description: После загрузки процессора функция вызывается один раз
	и инициализирует управляющие регистры процессора
	TMS320F28335/TMS320F28379D.

	Автор: Улитовский Д.И.
	Дата последнего обновления: 2021.10.04
**************************************************************************/


#include "init28335.h"

#define FREQ_TIMER_3    (FREQ_PWM*2)
#define MAX_U_PROC_SMALL	        2.5                 //1.4
#define MAX_U_PROC                  1.3             //1.11                 //1.4
#define MIN_U_PROC                  0.8       //0.7

#define ADD_U_MAX_GLOBAL            200.0   //V  Насколько поднимаем уставку GLOBAL относительно ZadanieU_Charge
#define ADD_U_MAX_GLOBAL_SMALL      500.0   //V  Насколько поднимаем уставку GLOBAL относительно ZadanieU_Charge
#define LEVEL_DETECT_U_SMALL        1000.0  //V  Насколько поднимаем уставку GLOBAL относительно ZadanieU_Charge

void init28335(void) {

	edrk.flag_second_PCH = 0;

	edrk_init_variables_matlab();
    init_global_time_struct(FREQ_TIMER_3);

    Init_Adc_Variables();

    //svgen_pwm24_1.phase_sequence = SIMULINK_SEQUENCE;
    //svgen_pwm24_2.phase_sequence = SIMULINK_SEQUENCE;

    edrk.zadanie.iq_Izad = _IQ(1);
    edrk.disable_alg_u_disbalance = 0;
    edrk.zadanie.iq_limit_power_zad = _IQ(1);
    //analog_zero.iqU_1 = 2048;
    //analog_zero.iqU_2 = 2048;
} //void init28335(void)

void edrk_init_variables_matlab(void)
{

	initVectorControl();
	InitXPWM(FREQ_PWM);
	InitPWM_Variables();

//#if(SENSOR_ALG==SENSOR_ALG_23550)
//	rotorInit();
//#endif
//#if(SENSOR_ALG==SENSOR_ALG_22220)
//	// 22220 
//	rotorInit_22220();
//#endif

	control_station.clear(&control_station);

	edrk_init_matlab();


	init_ramp_all_zadanie();
	//init_all_limit_koeffs();



    for (int i = 0; i < CONTROL_STATION_CMD_LAST; i++)
        control_station.array_cmd[CONTROL_STATION_TERMINAL_CAN][i] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][i];


    ramp_all_zadanie(2);

    set_zadanie_u_charge_matlab();
    init_Uin_rms();
}

void edrk_init_matlab(void)
{

    edrk.Uzad_max = _IQ(K_STATOR_MAX); // макс амплитуда в Км для минимального импульса = DEF_PERIOD_MIN_MKS
    edrk.iq_bpsi_normal = _IQ(BPSI_NORMAL / NORMA_FROTOR);
    //    edrk.iq_f_provorot = _IQ(F_PROVOROT/NORMA_FROTOR);

    //init_simple_scalar();

    //edrk.flag_enable_update_hmi = 1;



    edrk.Uzad_max = _IQ(K_STATOR_MAX); // макс амплитуда в Км для минимального импульса = DEF_PERIOD_MIN_MKS
    edrk.iq_bpsi_normal = _IQ(BPSI_NORMAL / NORMA_FROTOR);
    //   edrk.iq_bpsi_max = _IQ(BPSI_MAXIMAL/NORMA_FROTOR);
    //    edrk.iq_f_provorot = _IQ(F_PROVOROT/NORMA_FROTOR);

    init_simple_scalar();

    edrk.flag_enable_update_hmi = 1;


    edrk.zadanie.ZadanieU_Charge = 2500;
    edrk.zadanie.iq_ZadanieU_Charge = _IQ(2500 / NORMA_ACP);
    edrk.temper_limit_koeffs.sum_limit = _IQ(1);
    simple_scalar1.fzad_add_max = _IQ(FZAD_ADD_MAX);
    edrk.Mode_ScalarVectorUFConst = ALG_MODE_FOC_OBOROTS;

    edrk.zadanie.iq_power_zad = _IQ(1);
    edrk.zadanie.iq_oborots_zad_hz = _IQ(1);

    edrk.MasterSlave = MODE_MASTER;
    edrk.master_theta;
    edrk.master_Iq;
    edrk.iq_power_kw_another_bs = edrk.P_to_master;
    edrk.tetta_to_slave;
    edrk.Iq_to_slave;
    edrk.P_to_master;

    uf_alg.winding_displacement_bs1;

    //edrk.zadanie.iq_set_break_level = _IQ(2500 / NORMA_ACP);

    control_station.setup_time_detect_active[CONTROL_STATION_TERMINAL_RS232] = 50;


}

void set_zadanie_u_charge_matlab(void)
{

	//edrk.ZadanieU_Charge = edrk.ZadanieU_Charge_RS;

	//edrk.iq_ZadanieU_Charge = _IQ(edrk.ZadanieU_Charge/NORMA_ACP);

    if (edrk.zadanie.ZadanieU_Charge<=100)
    {
        edrk.iqMIN_U_ZPT = _IQ(-50.0/NORMA_ACP);
        edrk.iqMIN_U_IN = _IQ(-50.0/NORMA_ACP);
    }
    else
    {

        edrk.iqMIN_U_ZPT = _IQ(edrk.zadanie.ZadanieU_Charge*MIN_U_PROC/NORMA_ACP);
        edrk.iqMIN_U_IN = _IQ(edrk.zadanie.ZadanieU_Charge*MIN_U_PROC/NORMA_ACP);

    }

    if (edrk.zadanie.ZadanieU_Charge<LEVEL_DETECT_U_SMALL)
    {
	  edrk.iqMAX_U_ZPT_Predzaryad = _IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC_SMALL/NORMA_ACP);
	  edrk.iqMAX_U_ZPT_Global = edrk.iqMAX_U_ZPT_Predzaryad + _IQ(ADD_U_MAX_GLOBAL_SMALL/NORMA_ACP); // +500V

    }
    else
    {
      edrk.iqMAX_U_ZPT_Predzaryad = _IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC/NORMA_ACP);
      edrk.iqMAX_U_ZPT_Global = edrk.iqMAX_U_ZPT_Predzaryad + _IQ(ADD_U_MAX_GLOBAL/NORMA_ACP); // +200V

      if (edrk.iqMAX_U_ZPT_Global>U_D_MAX_ERROR_GLOBAL_2800)
          edrk.iqMAX_U_ZPT_Global = U_D_MAX_ERROR_GLOBAL_2800;
    }

    edrk.iqMAX_U_ZPT =  _IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC/NORMA_ACP);
    edrk.iqMAX_U_IN = _IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC/NORMA_ACP);


}


void init_ramp_all_zadanie(void)
{
    _iq rampafloat;

    // rmp_fzad
    edrk.zadanie.rmp_fzad.RampLowLimit = _IQ(-MAX_ZADANIE_F / NORMA_FROTOR); //0
    edrk.zadanie.rmp_fzad.RampHighLimit = _IQ(MAX_ZADANIE_F / NORMA_FROTOR);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_F));
    edrk.zadanie.rmp_fzad.RampPlus = rampafloat;
    edrk.zadanie.rmp_fzad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_fzad.DesiredInput = 0;
    edrk.zadanie.rmp_fzad.Out = 0;

    // rmp_oborots_hz
    edrk.zadanie.rmp_oborots_zad_hz.RampLowLimit = _IQ(MIN_ZADANIE_OBOROTS_ROTOR / 60.0 / NORMA_FROTOR); //0
    edrk.zadanie.rmp_oborots_zad_hz.RampHighLimit = _IQ(MAX_ZADANIE_OBOROTS_ROTOR / 60.0 / NORMA_FROTOR);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_OBOROTS_ROTOR));
    edrk.zadanie.rmp_oborots_zad_hz.RampPlus = rampafloat;
    edrk.zadanie.rmp_oborots_zad_hz.RampMinus = -rampafloat;

    edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;
    edrk.zadanie.rmp_oborots_zad_hz.Out = 0;


    //
    edrk.zadanie.rmp_Izad.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_Izad.RampHighLimit = _IQ(MAX_ZADANIE_I_M / NORMA_ACP);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_I_M));
    edrk.zadanie.rmp_Izad.RampPlus = rampafloat;
    edrk.zadanie.rmp_Izad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_Izad.DesiredInput = 0;
    edrk.zadanie.rmp_Izad.Out = 0;

    //
    edrk.zadanie.rmp_ZadanieU_Charge.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_ZadanieU_Charge.RampHighLimit = _IQ(MAX_ZADANIE_U_CHARGE / NORMA_ACP);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_U_CHARGE));
    edrk.zadanie.rmp_ZadanieU_Charge.RampPlus = rampafloat;
    edrk.zadanie.rmp_ZadanieU_Charge.RampMinus = -rampafloat;

    edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = 0;
    edrk.zadanie.rmp_ZadanieU_Charge.Out = 0;



    //
    edrk.zadanie.rmp_k_u_disbalance.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_k_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_K_U_DISBALANCE);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_K_U_DISBALANCE));
    edrk.zadanie.rmp_k_u_disbalance.RampPlus = rampafloat;
    edrk.zadanie.rmp_k_u_disbalance.RampMinus = -rampafloat;

    edrk.zadanie.rmp_k_u_disbalance.DesiredInput = 0;
    edrk.zadanie.rmp_k_u_disbalance.Out = 0;


    //
    edrk.zadanie.rmp_kplus_u_disbalance.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_kplus_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_KPLUS_U_DISBALANCE);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_KPLUS_U_DISBALANCE));
    edrk.zadanie.rmp_kplus_u_disbalance.RampPlus = rampafloat;
    edrk.zadanie.rmp_kplus_u_disbalance.RampMinus = -rampafloat;

    edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = 0;
    edrk.zadanie.rmp_kplus_u_disbalance.Out = 0;



    //
    edrk.zadanie.rmp_kzad.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_kzad.RampHighLimit = _IQ(MAX_ZADANIE_K_M);

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_K_M));
    edrk.zadanie.rmp_kzad.RampPlus = rampafloat;
    edrk.zadanie.rmp_kzad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_kzad.DesiredInput = 0;
    edrk.zadanie.rmp_kzad.Out = 0;





    //
    edrk.zadanie.rmp_powers_zad.RampLowLimit = _IQ(MIN_ZADANIE_POWER * 1000.0 / (NORMA_MZZ * NORMA_MZZ)); //0
    edrk.zadanie.rmp_powers_zad.RampHighLimit = _IQ(MAX_ZADANIE_POWER * 1000.0 / (NORMA_MZZ * NORMA_MZZ));

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_POWER));
    edrk.zadanie.rmp_powers_zad.RampPlus = rampafloat;
    edrk.zadanie.rmp_powers_zad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_powers_zad.DesiredInput = 0;
    edrk.zadanie.rmp_powers_zad.Out = 0;

    //
    edrk.zadanie.rmp_limit_powers_zad.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_limit_powers_zad.RampHighLimit = _IQ(MAX_ZADANIE_POWER * 1000.0 / (NORMA_MZZ * NORMA_MZZ));

    rampafloat = _IQ(1.0 / (2.0 * FREQ_PWM * T_NARAST_ZADANIE_POWER));
    edrk.zadanie.rmp_limit_powers_zad.RampPlus = rampafloat;
    edrk.zadanie.rmp_limit_powers_zad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;
    edrk.zadanie.rmp_limit_powers_zad.Out = 0;

    //


}

void ramp_all_zadanie(int flag_set_zero)
{
    //////////////////////////////////////////////
    if (flag_set_zero == 0)
        edrk.zadanie.rmp_Izad.DesiredInput = edrk.zadanie.iq_Izad;
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_Izad.DesiredInput = 0;
            edrk.zadanie.rmp_Izad.Out = 0;
        }
        else
            edrk.zadanie.rmp_Izad.DesiredInput = 0;

    edrk.zadanie.rmp_Izad.calc(&edrk.zadanie.rmp_Izad);
    edrk.zadanie.iq_Izad_rmp = edrk.zadanie.rmp_Izad.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = edrk.zadanie.iq_ZadanieU_Charge;
    edrk.zadanie.rmp_ZadanieU_Charge.calc(&edrk.zadanie.rmp_ZadanieU_Charge);
    edrk.zadanie.iq_ZadanieU_Charge_rmp = edrk.zadanie.rmp_ZadanieU_Charge.Out;
    //////////////////////////////////////////////
    if (flag_set_zero == 0)
        edrk.zadanie.rmp_fzad.DesiredInput = edrk.zadanie.iq_fzad;
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_fzad.DesiredInput = 0;
            edrk.zadanie.rmp_fzad.Out = 0;
        }
        else
            edrk.zadanie.rmp_fzad.DesiredInput = 0;

    edrk.zadanie.rmp_fzad.calc(&edrk.zadanie.rmp_fzad);
    edrk.zadanie.iq_fzad_rmp = edrk.zadanie.rmp_fzad.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_k_u_disbalance.DesiredInput = edrk.zadanie.iq_k_u_disbalance;
    edrk.zadanie.rmp_k_u_disbalance.calc(&edrk.zadanie.rmp_k_u_disbalance);
    edrk.zadanie.iq_k_u_disbalance_rmp = edrk.zadanie.rmp_k_u_disbalance.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = edrk.zadanie.iq_kplus_u_disbalance;
    edrk.zadanie.rmp_kplus_u_disbalance.calc(&edrk.zadanie.rmp_kplus_u_disbalance);
    edrk.zadanie.iq_kplus_u_disbalance_rmp = edrk.zadanie.rmp_kplus_u_disbalance.Out;
    //////////////////////////////////////////////
    if (flag_set_zero == 0)
        edrk.zadanie.rmp_kzad.DesiredInput = edrk.zadanie.iq_kzad;
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_kzad.DesiredInput = 0;
            edrk.zadanie.rmp_kzad.Out = 0;
        }
        else
            edrk.zadanie.rmp_kzad.DesiredInput = 0;
    edrk.zadanie.rmp_kzad.calc(&edrk.zadanie.rmp_kzad);
    edrk.zadanie.iq_kzad_rmp = edrk.zadanie.rmp_kzad.Out;
    //////////////////////////////////////////////
    if (flag_set_zero == 0)
        edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = edrk.zadanie.iq_oborots_zad_hz;
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;
            edrk.zadanie.rmp_oborots_zad_hz.Out = 0;
        }
        else
            edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;

    edrk.zadanie.rmp_oborots_zad_hz.calc(&edrk.zadanie.rmp_oborots_zad_hz);
    edrk.zadanie.iq_oborots_zad_hz_rmp = edrk.zadanie.rmp_oborots_zad_hz.Out;

    //////////////////////////////////////////////
    if (flag_set_zero == 0)
        edrk.zadanie.rmp_limit_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad;
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;
            edrk.zadanie.rmp_limit_powers_zad.Out = 0;
        }
        else
            edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;

    edrk.zadanie.rmp_limit_powers_zad.calc(&edrk.zadanie.rmp_limit_powers_zad);
    edrk.zadanie.iq_limit_power_zad_rmp = edrk.zadanie.rmp_limit_powers_zad.Out;



    //////////////////////////////////////////////
    if (flag_set_zero == 0)
    {
        if (edrk.zadanie.iq_power_zad > edrk.zadanie.iq_limit_power_zad_rmp)
            edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad_rmp;
        else
            edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_power_zad;
    }
    else
        if (flag_set_zero == 2)
        {
            edrk.zadanie.rmp_powers_zad.DesiredInput = 0;
            edrk.zadanie.rmp_powers_zad.Out = 0;
        }
        else
            edrk.zadanie.rmp_powers_zad.DesiredInput = 0;

    edrk.zadanie.rmp_powers_zad.calc(&edrk.zadanie.rmp_powers_zad);
    edrk.zadanie.iq_power_zad_rmp = edrk.zadanie.rmp_powers_zad.Out;




}