// ==========================================================
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 = (float)PHASE_EXCITIATION * DEG_TO_RAD;
gyro->Base_CompensationPhase = (float)PHASE_COMPENSATION * DEG_TO_RAD;
gyro->Base_PhiQPhase = (float)PHASE_PHIQ * DEG_TO_RAD;
gyro->ExcitationPhaseAG = gyro->Base_ExcitationPhaseAG;
gyro->ExcitationPhase = gyro->Base_ExcitationPhase;
gyro->CompensationPhase = gyro->Base_CompensationPhase;
gyro->PhiQPhase = gyro->Base_PhiQPhase;
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_ampl_1, (float)X_ampl*X_ampl, Kp_x1, Ki_x1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_ampl_2, (float) 0.0, Kp_x1, Ki_x1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_corr_1, (float) C_ampl, Kp_c1, Ki_c1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_corr_2, (float) 0.0, Kp_c1, Ki_c1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_quad_1, (float) Q_ampl, Kp_q1, Ki_q1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->pi_quad_2, (float) 0.0, Kp_q1, Ki_q1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk.pi_pll_x1, 0.0, Kp_f1, Ki_f1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk.pi_pll_x2, 0.0, Kp_f1, Ki_f1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk.pi_freq_1, 0.0, Kp_f1, Ki_f1, (float)DISCRET_TIME );
Init_PiReg_new( gyro->rmnk.pi_freq_2, 0.0, Kp_f1, Ki_f1, (float)DISCRET_TIME );
//Init_PiReg( &gyro->pi_om, 0.0, 600.0, 180.0, (float)-0.11, (float)DISCRET_TIME );
// -----------------
Init_CascadFiltration ( &gyro->cf_Freq );
Init_CascadFiltration ( &gyro->cf_Quad );
Init_CascadFiltration ( &gyro->cf_Temp );
Init_CascadFiltration ( &gyro->cf_Exci );
// -----------------
Init_PeriodDetector ( &gyro->rmnk.ANodePeriod, gyro->DefaultFrequency );
// ----------------- Задержки ------
Init_PhaseDetector( &gyro->Phase, gyro->DefaultFrequency);
// -------------------------
Init_RMNK( &gyro->rmnk, 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_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;
// ------ Демодуляторы ------
Init_ZForm( &gyro->fi_demod_c , 0.0 );
Init_ZForm( &gyro->fi_demod_q , 0.0 );
// ------ LowPass 500Hz ---- Чебышева 1 рода неравномерность в ПП 0.01 ( на частоте среза 100 герц фаза задержки 13 градусов )
gyro->fi_demod_c.a[0] = (float)(2.0 * 0.146146574414762 * 1.0e-003 );
gyro->fi_demod_c.a[1] = (float)(2.0 * 0.438439723244285 * 1.0e-003 );
gyro->fi_demod_c.a[2] = (float)(2.0 * 0.438439723244285 * 1.0e-003 );
gyro->fi_demod_c.a[3] = (float)(2.0 * 0.146146574414762 * 1.0e-003 );
gyro->fi_demod_c.b[0] = (float)( 1.000000000000000);
gyro->fi_demod_c.b[1] = (float)(-2.797716356193668);
gyro->fi_demod_c.b[2] = (float)( 2.617914892232589);
gyro->fi_demod_c.b[3] = (float)(-0.8190293634436032780);
// w0 = 5 HMax = 0715 -3dB = 34Hz
gyro->fi_demod_q.a[0] = (float)( 2.0 * 0.202050324201413 * 1e-5 );
gyro->fi_demod_q.a[1] = (float)( 2.0 * 0.404100648402826 * 1e-5 );
gyro->fi_demod_q.a[2] = (float)( 2.0 * 0.202050324201413 * 1e-5 );
gyro->fi_demod_q.a[3] = (float)( 0.0 );
gyro->fi_demod_q.b[0] = (float)( 1.000000000000000 );
gyro->fi_demod_q.b[1] = (float)(-1.996074539047090 );
gyro->fi_demod_q.b[2] = (float)( 0.996082621060058 );
gyro->fi_demod_q.b[3] = (float)( 0.0 );
// -----Амлитуда колебаний У ----
gyro->Y_CountSumValues = gyro->DefaultFrequency * 0.1;
gyro->Y_CountSumCounter = 0;
gyro->Y_CountSumValues_invert = (float)1.0 / (float)gyro->Y_CountSumValues;
gyro->Y_minVal = (float) 0.0;
gyro->Y_maxVal = (float) 0.0;
gyro->DebugCounter = (float)0.0;
gyro->DebugCoef = (float)1.0;
}
// ==========================================================
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;
}
