// ========================================================== 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; }