// ======================================================================================
void Init_RMNK( TRMNK *prmnk, float DefaultPeriod, float BaseXAmpl, float DefaultFreq )
{
// ----- Инициализация переменных ------
prmnk->x_phi = (float)0.0;
prmnk->delta_phi = (float)TWO_PI / DefaultPeriod;
prmnk->delta_phi_m = prmnk->delta_phi;
prmnk->Qx[0] = (float)0.0;
prmnk->Qx[1] = BaseXAmpl;
prmnk->Qx[2] = (float)0.0;
// ------ Значения MjuX могут меняться (это скорость сходимости) -----
prmnk->MjuX[0] = (float)0.002;
prmnk->MjuX[1] = (float)0.002;
prmnk->MjuX[2] = (float)0.002;
// ------ Значения MjuX могут меняться (это скорость сходимости) -----
prmnk->MjuY[0] = (float)0.004;
prmnk->MjuY[1] = (float)0.004;
prmnk->MjuY[2] = (float)0.004;
Init_LowPass_IRFiltr_1rstOrder( prmnk->fi_Xa, 250.0, BaseXAmpl, DISCRET_TIME );
Init_LowPass_IRFiltr_1rstOrder( prmnk->fi_Xb, 250.0, (float)0.0, DISCRET_TIME );
Init_LowPass_IRFiltr_1rstOrder( prmnk->fi_Ya, 250.0, (float)0.0, DISCRET_TIME );
Init_LowPass_IRFiltr_1rstOrder( prmnk->fi_Yb, 250.0, (float)0.0, DISCRET_TIME );
}
// ==========================================================
void Init_Algo( TGyro *gyro, float *apUARTfloats )
{
Sint16 iCounter = 0;
gyro->pUARTfloats = apUARTfloats;
// --- Переприсвоение Дефайнов -----
gyro->DefaultFrequency = (float)Base_Frequency;
gyro->DefaultPeriod = (float)ALGO_FREQUENCY / gyro->DefaultFrequency;
gyro->Frequency = gyro->DefaultFrequency;
gyro->k_Omega = k_omega;
gyro->DebugTime = TIMES;
gyro->DebugMode = DEBUG_MODE;
gyro->StartUpTime = (Sint32)(GYRO_StartUp_Time * ALGO_FREQUENCY);
/////////////
gyro->isXPhase_corr = CORR_X_PHASE;
gyro->isYPhase_corr = CORR_Y_PHASE;
gyro->isAlfaE_f_corr = CORR_ALFA_E_f;
gyro->isAlfaQ_f_corr = CORR_ALFA_Q_f;
gyro->isAlfaE_t_corr = CORR_ALFA_E_t;
gyro->isAlfaQ_t_corr = CORR_ALFA_Q_t;
gyro->isPhiQ_corr = CORR_PhiQ;
gyro->isStatic1_corr = CORR_STATIC_1;
gyro->isStatic2_corr = CORR_STATIC_2;
gyro->isDynamic_1_corr = CORR_DYNAMIC_1;
gyro->isDynamic_2_corr = CORR_DYNAMIC_2;
/////////////
gyro->GyroOut_corr_st1 = (float)0.0;
gyro->GyroOut_corr_st2 = (float)0.0;
gyro->GyroOut_corr_st1_a = (float)0.0;
gyro->GyroOut_corr_st1_b = (float)0.0;
gyro->GyroOut_corr_st1_c = (float)0.0;
gyro->GyroOut_corr_st2_a = (float)0.0;
gyro->GyroOut_corr_st2_b = (float)0.0;
gyro->GyroOut_corr_st2_c = (float)0.0;
gyro->GyroOut_MK1 = (float)1.0; // Единица
gyro->GyroOut_MK2 = (float)1.0; // Единица
gyro->corr_cos_alfa_e = (float)1.0; // Единица
gyro->corr_sin_alfa_e = (float)0.0;
gyro->corr_cos_alfa_q = (float)1.0; // Единица
gyro->corr_sin_alfa_q = (float)0.0;
gyro->CorrectionOrderCalc = 0;
/////////////
gyro->Base_ExcitationPhaseAG = (float)PHASE_AG * DEG_TO_RAD;
gyro->Base_ExcitationPhase_x = (float)PHASE_EXCITIATION_X * DEG_TO_RAD;
gyro->Base_ExcitationPhase_y = (float)PHASE_EXCITIATION_Y * DEG_TO_RAD;
gyro->ExcitationPhaseAG = gyro->Base_ExcitationPhaseAG;
gyro->ExcitationPhase_x = gyro->Base_ExcitationPhase_x;
gyro->ExcitationPhase_y = gyro->Base_ExcitationPhase_y;
gyro->MainSum_counter = 0;
gyro->MainSumInit_counter = 0;
gyro->isMainSumInited = 0;
// -- ФАЗА (Задержка) АГ ----
gyro->Zn_exc.zn_curr = (ALGO_FREQUENCY / Base_Frequency) * gyro->ExcitationPhaseAG * TWO_PI_INVERT;
Init_Zn( &gyro->Zn_exc );
// -------- Инит коррекционных переменных --------
gyro->f1 = (float)0.0; gyro->e1 = (float)0.0; gyro->q1 = (float)0.0; gyro->t1 = (float)0.0;
gyro->f2 = (float)0.0; gyro->e2 = (float)0.0; gyro->q2 = (float)0.0; gyro->t2 = (float)0.0;
gyro->f3 = (float)0.0; gyro->e3 = (float)0.0; gyro->q3 = (float)0.0; gyro->t3 = (float)0.0;
gyro->f4 = (float)0.0; gyro->e4 = (float)0.0; gyro->q4 = (float)0.0; gyro->t4 = (float)0.0;
gyro->f5 = (float)0.0; gyro->e5 = (float)0.0; gyro->q5 = (float)0.0; gyro->t5 = (float)0.0;
gyro->f6 = (float)0.0; gyro->e6 = (float)0.0; gyro->q6 = (float)0.0; gyro->t6 = (float)0.0;
// ----------------- Пегуляторы ------
Init_PiReg_new( gyro->pi_x_1, X_ampl*X_ampl, Kp_x , Ki_x , (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_x_2, 0.0, Kp_x , Ki_x , (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_y_1, Y_ampl*Y_ampl, Kp_y , Ki_y , (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_y_2, 0.0, Kp_y , Ki_y , (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_phi_1, 0.0, Kp_phi, Ki_phi, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_phi_2, 0.0, Kp_phi, Ki_phi, (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk_x.pi_pll_x1, 0.0, Kp_f , Ki_f , (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk_x.pi_pll_x2, 0.0, Kp_f , Ki_f , (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk_y.pi_pll_x1, 0.0, Kp_f , Ki_f , (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk_y.pi_pll_x2, 0.0, Kp_f , Ki_f , (float)DISCRET_TIME );
// -----------------
//Init_CascadFiltration ( &gyro->cf_Freq );
//Init_CascadFiltration ( &gyro->cf_Temp );
//Init_CascadFiltration ( &gyro->cf_NCycles );
//Init_CascadFiltration ( &gyro->cf_Quad );
//Init_CascadFiltration ( &gyro->cf_Exci );
// -----------------
Init_PeriodDetector ( &gyro->rmnk_x.ANodePeriod, gyro->DefaultFrequency );
// -----------------
// Init_PhaseDetector( &gyro->Phase, gyro->DefaultFrequency);
// -------------------------
Init_RMNK( &gyro->rmnk_x, gyro->DefaultPeriod, (float)X_ampl*X_ampl, gyro->DefaultFrequency );
Init_RMNK( &gyro->rmnk_y, gyro->DefaultPeriod, (float)X_ampl*X_ampl, gyro->DefaultFrequency );
gyro->Temperature = (float)0.0;
gyro->NULL_value = (float)0.0;
gyro->in_Ux = (float)0.0;
gyro->in_Uy = (float)0.0;
gyro->out_Ux = (float)0.5;
gyro->out_Uy = (float)0.0;
gyro->KXPhase = (float)0.0;
gyro->K_PhaseScan = (float)1.0;
// -------------------------
Init_DebugMode ( gyro, &Debug );
Init_BandPassFiltration_new ( gyro->fi_input_x, gyro->DefaultFrequency, 100.0, (float)DISCRET_TIME );
Init_BandPassFiltration_new ( gyro->fi_input_y, gyro->DefaultFrequency,400.0, (float)DISCRET_TIME ); // Фаза задержки на 100 Гц 7 градусов
//Init_BandPassFiltration_new ( gyro->fi_input_y, gyro->DefaultFrequency,1000.0, (float)DISCRET_TIME ); // Фаза задержки на 100 Гц 11.5 градусов
//Init_BandPassFiltration_new ( gyro->fi_input_y, gyro->DefaultFrequency, 800.0, (float)DISCRET_TIME ); // Фаза задержки на 100 Гц 14 градусов
Init_LowPass_IRFiltr_1rstOrder( gyro->fi_Phase, 50.0, (float)0.0, (float)1.0 / (float)gyro->DefaultFrequency );
Init_UART_Param ( gyro );
// ------- Второстепенные иниты -------
gyro->AlgoCounter = 0;
gyro->isAlgoCounter = 1;
gyro->CurDebugTime = (float)0.0;
// ---------
#pragma vector_for
for ( iCounter = 0; iCounter < SLIDER_BUFFERS_LENGTH; iCounter++)
gyro->XBuffer[iCounter] = (float)0.0;
// ------
gyro->DebugCounter = (float)0.0;
gyro->DebugCoef = (float)1.0;
gyro->BaseAntinodePhase = (float)0.0;
gyro->kPhase = (float)1.0;
}
