/*
 * alg_simple_scalar.c
 *
 *  Created on: 26 èþí. 2020 ã.
 *      Author: Yura
 */


#include <alg_simple_scalar.h>
#include <edrk_main.h>
#include <log_to_mem.h>
#include <master_slave.h>
#include <math.h>
#include <params_alg.h>
#include <params_norma.h>
#include <project.h>
#include "log_to_mem.h"
#include "IQmathLib.h"
#include "math_pi.h"
#include "mathlib.h"
#include "params_pwm24.h"



#pragma DATA_SECTION(simple_scalar1,".slow_vars");
ALG_SIMPLE_SCALAR simple_scalar1 = ALG_SIMPLE_SCALAR_DEFAULT;



void init_simple_scalar(void)
{
    simple_scalar1.mzz_add_1 = _IQ(MZZ_ADD_1/NORMA_MZZ);
    simple_scalar1.mzz_add_2 = _IQ(MZZ_ADD_2/NORMA_MZZ);
    simple_scalar1.poluses  =  _IQ(POLUS);
    simple_scalar1.iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);

    simple_scalar1.powerzad_add = _IQ(POWERZAD_ADD_MAX);
    simple_scalar1.powerzad_dec = _IQ(POWERZAD_DEC);

    simple_scalar1.k_freq_for_pid = _IQ(1.0);
    simple_scalar1.k_freq_for_pid = _IQ(450.0/FREQ_PWM);

    simple_scalar1.iq_add_kp_df = _IQ(ADD_KP_DF);
    simple_scalar1.iq_add_ki_df = _IQ(ADD_KI_DF);

    simple_scalar1.min_mzz_for_df = _IQ(MIN_MZZ_FOR_DF/NORMA_MZZ);

    simple_scalar1.pidF_Kp = _IQ(PID_KP_F);
    simple_scalar1.pidF_Ki = _IQ(PID_KI_F);



    simple_scalar1.pidIm1.Kp=_IQ(PID_KP_IM);
    simple_scalar1.pidIm1.Ki=_IQ(PID_KI_IM);

    simple_scalar1.pidIm_Ki = simple_scalar1.pidIm1.Ki;

    simple_scalar1.pidIm1.Kc=_IQ(PID_KC_IM);
    simple_scalar1.pidIm1.Kd=_IQ(PID_KD_IM);


    simple_scalar1.pidIm1.OutMax=_IQ(K_STATOR_MAX);
    simple_scalar1.pidIm1.OutMin=_IQ(0);

//////////////


    simple_scalar1.pidF.Kp=_IQ(PID_KP_F);
    simple_scalar1.pidF.Ki=_IQ(PID_KI_F);
    simple_scalar1.pidF.Kc=_IQ(PID_KC_F);
    simple_scalar1.pidF.Kd=_IQ(PID_KD_F);


    simple_scalar1.pidF.OutMax=_IQ(500/NORMA_MZZ);
    simple_scalar1.pidF.OutMin=_IQ(0);
 //   iq_MAX_DELTA_pidF = _IQ(MAX_DELTA_pidF/NORMA_WROTOR);
/////////////////////////
//    simple_scalar1.pidPower_Kp = _IQ(PID_KP_POWER);
//    simple_scalar1.pidPower_Ki = _IQ(PID_KI_POWER);


 //   iq_add_kp_dpower = _IQ(ADD_KP_DPOWER);
 //   iq_add_ki_dpower = _IQ(ADD_KI_DPOWER);

    simple_scalar1.pidPower.Kp=_IQ(PID_KP_POWER);
    simple_scalar1.pidPower.Ki=_IQ(PID_KI_POWER);
    simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid); // ââîäèì êîððåêöèþ Ki ïî ÷àñòîòå ØÈÌà

    simple_scalar1.pidPower.Kc=_IQ(PID_KC_POWER);
    simple_scalar1.pidPower.Kd=_IQ(PID_KD_POWER);

    simple_scalar1.pidPower.OutMax=_IQ(500/NORMA_MZZ);
    simple_scalar1.pidPower.OutMin=_IQ(0);


    simple_scalar1.iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%


    // ìèí. ñêîëüæåíèå
    simple_scalar1.min_bpsi = _IQ(0.05/NORMA_FROTOR);

}

/***************************************************************/
/* Ï/ï óïðàâëåíèy   ïñåâäî âåêòîð 3 -
   vector_moment(real Frot,    - îáîðîòû ðîòîðà
                 real fzad,    - çàäàííûå îáîðîòû ðîòîðà
                 real mzz_zad, - ìàêñèìàëüíûé âîçìîæíûé ìîìåíò-îãðàíè÷åíèå òîêà
                 real *Fz,     - ðåçóëüòàò ðàñ÷åòà - ÷àñòîòà íàïðyæåíèy â ñòàòîðå
                 real *Uz1,    - ðåçóëüòàò ðàñ÷åòà - êîýô. ìîäóëyöèè â 1 îáìîòêå ñòàòîðà
                 real *Uz2)     - ðåçóëüòàò ðàñ÷åòà - êîýô. ìîäóëyöèè â 2 îáìîòêå ñòàòîðà


   Èñïîëüçóåòñy ðåãóëyòîð ñêîðîñòè, êîòîðûé âûïîëíyåò çàäàíèå  fzad ïî ñêîðîñòè

   Çàâîäèòñy ïîñòîyííîå ñêîëüæåíèå = 0.5

   Èäåò ðàñ÷åò íàïðyæåíèy ÷åðåç ìîäóëü òîêà ïî îäíîé èç 3-õ ôàçíîé ñòîåê.
   Çàìûêàåòñy îáðàòíày ñâyçü ïî îáîðîòàì          */
/****************************************************************/
//#pragma CODE_SECTION(simple_scalar,".fast_run");
void simple_scalar(int n_alg, int n_wind_pump,
                   _iq Frot_pid,_iq Frot, _iq fzad,
                   _iq iqKoefOgran,
                   _iq mzz_zad, _iq bpsi_const,
                    _iq iqIm, _iq iqUin, _iq Iin, _iq powerzad, _iq power_pid,
                    _iq Izad_from_master, int master,
                     _iq *Fz, _iq *Uz1, _iq *Uz2, _iq *Izad_out)
{

   _iq mzz,  dF, dI1, Izad, Uz_t1, Kpred_Ip, pred_Ip;//, znak_moment;
   _iq dI2, Uz_t2;

   _iq pkk=0,ikk=0;
   _iq Im_regul=0;


   static _iq bpsi=0;
 //  static _iq IQ_POLUS=0;


   static _iq mzz_zad_int=0;
   static _iq mzzi=0;

   static _iq I1_i=0;
   static _iq I2_i=0;

   static _iq Im1=0;
   static _iq Im2=0;

   static _iq Uze_t1=0;
   static _iq Uze_t2=0;

 //  static _iq fzad_ogr=0;



//   static _iq koef_Uz_t_filter=0;
   static _iq dI1_prev=0;
   static _iq Uz_t1_prev=0;

   static _iq dF_prev = 0;
   static _iq mzz_prev = 0;

 //  static _iq mzz_add_1, mzz_add_2;

   static _iq fzad_int=0;//, fzad_add_max;//,iq_mzz_max_for_fzad ;
   static _iq fzad_add=0; //fzad_dec
   _iq  halfIm1, halfIm2;

   static _iq powerzad_int=0,  powerzad_add_max=0 ;
//powerzad_dec powerzad_add
//   static _iq koef_bpsi=0;
 //  static _iq min_bpsi=0;
   static int flag_uz_t1=0;

 //  static _iq correct_err=0;
//   static _iq iq_dF_min1=0;
//   static _iq iq_dF_min2=0;
   _iq pred_dF,Kpred_dF;
   static _iq dF_PREDEL_LEVEL2 = 0,dF_PREDEL_LEVEL1=0;
   _iq Uze_ogr=0;

 //  static _iq iq_spad_k=1;
   static _iq myq_temp=0;
  // static _iq mzz_int_level1_on_F=0;


//     mzz = _IQsat(mzz,mzz_zad_int,0);


   simple_scalar1.mzz_zad = mzz_zad;
   simple_scalar1.Izad_from_master = Izad_from_master;


   /* óñòàíàâëèâàåì íà÷àëüíûå óñëîâèy âñåõ ðåãóëyòîðîâ */
   if ( (Frot==0) && (fzad==0) )
   {
      mzzi = 0;
      fzad_int = 0;
      powerzad_int = 0;

   }

   if (mzz_zad==0)
   {
      mzz=0;
      I1_i=0;
      mzzi=0;
      mzz_zad_int = 0;
      fzad_int = 0;
      powerzad_int = 0;

      simple_scalar1.pidIm1.Up1 = 0;
      simple_scalar1.pidIm1.Ui = 0;

      simple_scalar1.pidF.Up1 = 0;
      simple_scalar1.pidF.Ui = 0;

      simple_scalar1.pidPower.Up1 = 0;
      simple_scalar1.pidPower.Ui = 0;

      Uze_t1=0;
      Uze_t2=0;

      dI1_prev = 0;
      Uz_t1_prev = 0;

      dF_prev = 0;
      mzz_prev = 0;


      // çàäàò÷èê èíòåíñèâíîñòè fzad
      fzad_add = _IQ(FZAD_ADD_MAX/NORMA_FROTOR);
      // çàäàò÷èê èíòåíñèâíîñòè fzad ïî ìèíóñó
 //     fzad_dec = _IQ(FZAD_DEC/NORMA_FROTOR);
//
//
      // çàäàò÷èê èíòåíñèâíîñòè mzz_max
  //    iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);


      // êîýô. ôèëüòðà Uz_t_filter
  //    koef_Uz_t_filter = _IQ(0.001/0.5);  //0.0333



      // êîýô. ðàñ÷åòà ñêîëüæåíèå îò mzz
 //     koef_bpsi = _IQ((0.6/NORMA_WROTOR)/(200.0/NORMA_MZZ));

      flag_uz_t1=0;


      // êîýô. óñèëåíèß äëß èíòåãðàëüíîãî àäàïòèâíîãî êîýô. â ðåãóëßòîðå ñêîðîñòè
 //     correct_err = _IQ(2.5/NORMA_WROTOR);

      // ìèí. óðîâåíü äëß ðàáîòû êîýô. óñèëåíèß äëß èíòåãðàëüíîãî àäàïòèâíîãî êîýô. â ðåãóëßòîðå ñêîðîñòè
  //    iq_dF_min1 = _IQ(1.0/NORMA_WROTOR);

   //   iq_dF_min2 = _IQ(1.5/NORMA_WROTOR);

      // êîýô. çàäàíèß ñêîðîñòè ñïàäà Km
  //    iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%

//    iq_spad_k = _IQ(0.9965); //0.993 ~ 0.4 sek äî 5%


//      dF_PREDEL_LEVEL1 = _IQ(0.5/NORMA_WROTOR);
//    dF_PREDEL_LEVEL2 = _IQ(1.5/NORMA_WROTOR);

   //   mzz_int_level1_on_F = _IQ(1.0/NORMA_WROTOR);
//      mzz_int_level2_on_F = _IQ(1.5/NORMA_WROTOR);


   }


   /* çàäàò÷èê èíòåíñèâíîñòè ìîìåíòà */
   if (n_alg==1)
   {

       mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);

//     if (Frot_pid>mzz_int_level1_on_F)
//        mzz_zad_int = zad_intensiv_q(mzz_add_1, mzz_add_1, mzz_zad_int, mzz_zad);
//     else
//        mzz_zad_int = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz_zad_int, mzz_zad);

   }


   if (n_alg==2)
     mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);

//   myq_temp = _IQdiv(mzz_zad, simple_scalar1.iq_mzz_max_for_fzad);

//   myq_temp = _IQmpy( myq_temp, fzad_add_max);

 //  fzad_add = myq_temp;


   fzad_int = zad_intensiv_q(fzad_add, fzad_add, fzad_int, fzad);

   powerzad_int = zad_intensiv_q(simple_scalar1.powerzad_add, simple_scalar1.powerzad_add, powerzad_int, powerzad);

   if (n_alg==1)
   {
    /* ðåãóëyòîð ñêîðîñòè */
    if (mzz_zad_int>=0)
    {
      dF = fzad_int - Frot_pid;

//////////  Power PI //////////////

      simple_scalar1.pidPower.OutMax=mzz_zad;

//    pidPower.Kp = _IQmpy(  _IQdiv(iq_add_kp_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Kp);
//    pidPower.Ki = _IQmpy(  _IQdiv(iq_add_ki_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Ki);

//      simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid);


      simple_scalar1.pidPower.Ref = powerzad_int;

      simple_scalar1.pidPower.Fdb = power_pid;
      simple_scalar1.pidPower.calc(&simple_scalar1.pidPower);


    // Saturate the integral output

       if (simple_scalar1.pidPower.Ui > simple_scalar1.pidPower.OutMax)
           simple_scalar1.pidPower.Ui =  simple_scalar1.pidPower.OutMax;
       else if (simple_scalar1.pidPower.Ui < simple_scalar1.pidPower.OutMin)
           simple_scalar1.pidPower.Ui =  simple_scalar1.pidPower.OutMin;


//////////////////////////////
//////////////////////////////


      // îãðàíè÷åíèå ìàêñ. çíà÷èíèß âûõîäà ðåãóëßòîðà
    //   pidF.OutMax=mzz_zad_int;
          // èëè òàê

       simple_scalar1.pidF.OutMax = simple_scalar1.pidPower.Out;

//    pidF.OutMax = mzz_zad;

       simple_scalar1.pidF.Kp = _IQmpy(  _IQdiv(simple_scalar1.iq_add_kp_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Kp);
       simple_scalar1.pidF.Ki = _IQmpy(  _IQdiv(simple_scalar1.iq_add_ki_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Ki);

       simple_scalar1.pidF.Ki = _IQmpy(simple_scalar1.pidF.Ki,simple_scalar1.k_freq_for_pid);

/////////////////////////

//      if (_IQabs(dF)<iq_dF_min1)
//      {
//         dF=iq_dF_min1;
//         m.m1.bit.w_rotor_ust = 1;
//      }
//
//      if (_IQabs(dF)>iq_dF_min2)
//      {
//         m.m1.bit.w_rotor_ust = 0;
//      }
//////////////////////////////////

      // áåç êîððåêöèé dF
      simple_scalar1.pidF.Ref = fzad_int;

      simple_scalar1.pidF.Fdb = Frot_pid;
      simple_scalar1.pidF.calc(&simple_scalar1.pidF);


    // Saturate the integral output

       if (simple_scalar1.pidF.Ui > simple_scalar1.pidF.OutMax)
           simple_scalar1.pidF.Ui =  simple_scalar1.pidF.OutMax;
       else if (simple_scalar1.pidF.Ui < simple_scalar1.pidF.OutMin)
           simple_scalar1.pidF.Ui =  simple_scalar1.pidF.OutMin;
/////////////////////////////////////

       mzz = simple_scalar1.pidF.Out;

///////////////////////////////////////



// çàäàò÷èê èíòåíñèâíîñòè íà òîê
//       mzz = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz, pidF.Out);

//       mzzi = zad_intensiv_q(mzz_add_2, mzz_add_2, mzzi, mzz);

       // îãðàíè÷èëè äèàïàçîí mzz
       mzz = _IQsat(mzz,mzz_zad_int,0);

    }
    else
    {
       mzz = 0;
    }

   }

   if (n_alg==2)
   {
      mzz = mzz_zad_int;
   }


   if (master == MODE_SLAVE)
   {
       mzz = Izad_from_master;
       // îãðàíè÷èëè äèàïàçîí mzz
       mzz = _IQsat(mzz,mzz_zad_int,0);
   }


   *Izad_out = mzz;
   /* ïðè ïðåâûøåíèè  òîêà íåêîòîðîãî ïîðîãîâîãî çíà÷åíèy I_PREDEL_LEVEL1
      íà÷èíàåì ëèíåéíî ñáàâëyòü ìîìåíò */

/*
   pred_Ip = (filter.I_3+filter.I_6)-I_PREDEL_LEVEL1;


   if  (pred_Ip<0)
      Kpred_Ip=0.0;  // âñå â íîðìå
   else
   {
     // ïîðà îãðàíè÷èâàòü ìîìåíò
     if  (pred_Ip>=(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1))
          Kpred_Ip=1;
     else
          Kpred_Ip = pred_Ip/(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1);

   }

   // à âîò è îãðàíè÷åíèå
   Izad = mzz * (1-Kpred_Ip);
  */


   iqKoefOgran = _IQsat(iqKoefOgran,CONST_IQ_1,0);
   Izad = _IQmpy(mzz, iqKoefOgran);

//   if ((n_alg==1) || (n_alg==2))
//   {
//
//     Im1 = iqIm_1;
//     Im2 = iqIm_2;
//
//    if (n_wind_pump==0) // ðàáîòà ïî äâóì îáìîòêàì
//    {
//
//     halfIm1 = Im1 >> 1;
//     halfIm2 = Im2 >> 1;
//
//     if (Im1>halfIm2) //if (Im1>IQdiv(Im2,_IQ(2.0)))
//     {
//       Im_regul=Im1;
//       simple_scalar1.UpravIm1=1;
//       simple_scalar1.UpravIm2=0;
//     }
//     else
//     {
//       if (Im2>halfIm1)
//       {
//          Im_regul=Im2;
//          simple_scalar1.UpravIm2=1;
//          simple_scalar1.UpravIm1=0;
//       }
//       else
//       {
//          Im_regul=Im1;  //Im1
//          simple_scalar1.UpravIm1=1;//1
//          simple_scalar1.UpravIm2=0;//0
//       }
//     }
//    }
//
//    if (n_wind_pump==1)  // íà íàñîñå 1 îáìîòêà çíà÷èò áåðåì òîê ñ 2-îé
//    {
//       Im_regul=Im2;
//       simple_scalar1.UpravIm1=0;
//       simple_scalar1.UpravIm2=1;
//    }
//
//    if (n_wind_pump==2) // íà íàñîñå 2 îáìîòêà çíà÷èò áåðåì òîê ñ 1-îé
//    {
//       Im_regul=Im1;
//       simple_scalar1.UpravIm1=1;
//       simple_scalar1.UpravIm2=0;
//    }

     Im_regul = iqIm;

     simple_scalar1.Im_regul = Im_regul;
     simple_scalar1.Izad = Izad;

     dI1 =  (Izad - Im_regul );

     simple_scalar1.pidIm1.Ki = simple_scalar1.pidIm_Ki;
     simple_scalar1.pidIm1.Ki = _IQmpy(simple_scalar1.pidIm1.Ki,simple_scalar1.k_freq_for_pid);


     simple_scalar1.pidIm1.Ref = _IQdiv(Izad,iqUin);
     simple_scalar1.pidIm1.Fdb = _IQdiv(Im_regul,iqUin);
     simple_scalar1.pidIm1.calc(&simple_scalar1.pidIm1);


     Uz_t1 = simple_scalar1.pidIm1.Out;

    // îãðàíè÷åíèå ñïàäà Km
     if (Uz_t1<Uze_t1)
     {
         Uze_ogr =_IQmpy(Uze_t1, simple_scalar1.iq_spad_k);
         if (Uze_ogr>Uz_t1) Uze_t1 = Uze_ogr;
         else
            Uze_t1 = Uz_t1;
     }
     else
     {
          Uze_t1 = Uz_t1;
     }

     Uze_t1 = _IQsat(Uze_t1,simple_scalar1.pidIm1.OutMax,0);

//   }


   /* ðåçóëüòàòû îòäàåì íàðóæó */
   *Uz1 =  Uze_t1;
   *Uz2 =  Uze_t1;





      bpsi = bpsi_const;

   // ñêîëüæ. ~ ìîìåíòó
//    bpsi = _IQmpy(koef_bpsi,mzz);

      bpsi = _IQsat(bpsi,bpsi_const, simple_scalar1.min_bpsi);

#ifdef BAN_ROTOR_REVERS_DIRECT
// èñïîëüçóåì çàùèòó îò íåïðàâèëüíîãî âðàùåíèß
      if (analog.filter_direct_rotor==-1)
      // êðóòèìñß â äðóãóþ ñòîðîíó, ïîýòîìó ïûòàåìñß ïåðåéòè íà ñêîëüæåíèè â íîðìàëüíîå âðàùåíèå
        *Fz = bpsi;
      else
      // âñå íîðìàëüíî, íàïðàâëåíèå ïðàâèëüíîå
        *Fz = _IQmpy(Frot,IQ_POLUS) + bpsi;

#else

      *Fz = _IQmpy(Frot,simple_scalar1.poluses) + bpsi;      /* bpsi - ñêîëüæåíèå, áåðåì ïîêà
                                     êîíñòàíòîé õîòy òîæå äîëæåí ðåãóëèðîâàòüñy  */

#endif




//   if (n_alg==2)
//   {
//
//      *Fz = fzad_provorot;
//                                /* bpsi - ñêîëüæåíèå, áåðåì ïîêà
//                                   êîíñòàíòîé õîòy òîæå äîëæåí ðåãóëèðîâàòüñy  */
//   }


//   logpar.log1 = (int16)(_IQtoIQ15(Izad));
//   logpar.log2 = (int16)(_IQtoIQ15(mzz_zad));//(int16)(_IQtoIQ15(Uze_t1));
//   logpar.log3 = (int16)(_IQtoIQ15(fzad_int));
//   logpar.log4 = (int16)(_IQtoIQ15(simple_scalar1.pidF.Ui));
//   logpar.log5 = (int16)(_IQtoIQ14(simple_scalar1.pidF.Up));
//   logpar.log6 = (int16)(_IQtoIQ14(simple_scalar1.pidF.SatErr));
//   logpar.log7 = (int16)(_IQtoIQ15(mzz_zad_int));
//   logpar.log8 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ref));
//   logpar.log9 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Fdb));
//   logpar.log10 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ui));
//   logpar.log11 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Up));



}