示例#1
0
// *****************************************************************
// 
// *****************************************************************
void Run_Algo( TGyro *gyro ) 
{
    float cos_freq, sc_argument, local_var;
    Sint8 iCounter;

    // --- 1 ------ НАСРОЙКА ПРИБОРА -----
    if ( gyro->DebugMode != 0 ) 
    {
        SetupAlgo( gyro );

        gyro->ExcitationPhaseAG = gyro->Base_ExcitationPhaseAG + gyro->c5;
        gyro->ExcitationPhase   = gyro->Base_ExcitationPhase   + gyro->c6;
        gyro->PhiQPhase         = gyro->PhiQPhase              - gyro->c10;

        if ( gyro->ExcitationPhaseAG < (float)0.0 ) gyro->ExcitationPhaseAG = (float)0.0;
        if ( gyro->ExcitationPhase   < (float)0.0 ) gyro->ExcitationPhase   = (float)0.0;
        //if ( gyro->PhiQPhase         < (float)-1.0 ) gyro->PhiQPhase         = (float)-1.0;

        // ----- Если сканироваение Стартовой фазы, зануляю УЗЕЛ ------
        if ( gyro->DebugMode == 6 ) gyro->in_Uy = 0;
    }

   // --- 2 ------ ДАННЫЕ В UART -------
    Setup_UART_data( gyro );

   // --- 3 ------ Фильтрация входных сигналов ------
    Run_BandPassFiltration_new( gyro->fi_input_x, gyro->in_Ux );
    Run_BandPassFiltration_new( gyro->fi_input_y, gyro->in_Uy );
    //gyro->fi_input_y[0] = gyro->in_Uy;

   // --- 4 ------ RMNK ------ (R-рекуррентный M-метод N-наименьших K-квадратов)
    Run_Demod_RMNK( &gyro->rmnk, gyro->fi_input_x[0], gyro->fi_input_y[0] );
    //Run_Demod_RMNK( &gyro->rmnk, gyro->in_Ux );

   // --- 5 ------ Амплитуда КОЛебаний канала У -----
/*   
    if ( gyro->Y_minVal > gyro->fi_input_y[0] ) gyro->Y_minVal = gyro->fi_input_y[0];
    if ( gyro->Y_maxVal < gyro->fi_input_y[0] ) gyro->Y_maxVal = gyro->fi_input_y[0];
    if ( ++gyro->Y_CountSumCounter >= gyro->Y_CountSumValues )
    {
        gyro->rmnk.YAmpl =  (float)0.5 * (gyro->Y_maxVal - gyro->Y_minVal);
        gyro->Y_CountSumCounter = 0;
        gyro->Y_minVal = (float) 0.0;
        gyro->Y_maxVal = (float) 0.0;
    }
*/   
   // --- 6 ------- Демодуляция Кориолиса и Квадратуры ---
    //Run_Demod( &gyro->fi_demod_c, - gyro->fi_input_y[0] * gyro->rmnk.x_base_sin );
    //Run_Demod( &gyro->fi_demod_q, - gyro->fi_input_y[0] * cosf( gyro->rmnk.x_phi + gyro->PhiQPhase ) );//
    //gyro->fi_demod_c.BlockOut = -gyro->rmnk.Qy[1];
    //gyro->fi_demod_q.BlockOut = -gyro->rmnk.Qy[2];
    gyro->fi_demod_c.BlockOut = -gyro->rmnk.fi_Ya[0];
    gyro->fi_demod_q.BlockOut = -gyro->rmnk.fi_Yb[0];
    

  // --- 7 ----- Регулирование колебаний амплитуды пучности ----
    Run_PiReg_new( gyro->pi_ampl_1,  gyro->rmnk.fi_Xa[0] );  
    Run_PiReg_new( gyro->pi_ampl_2, -gyro->pi_ampl_1[0]  );

  // --- 8 ------ Если автогениратор отключился 
  // ---   ------- тогда переходим на компенсирование узла
    if ( gyro->isAlgoCounter >= 2 ) 
    { 
        Run_PiReg_new( gyro->pi_corr_1,  gyro->fi_demod_c.BlockOut );   
        Run_PiReg_new( gyro->pi_corr_2, -gyro->pi_corr_1[0] );
        //gyro->pi_corr_2[0] = gyro->pi_corr_1[0];
       
        Run_PiReg_new( gyro->pi_quad_1,  gyro->fi_demod_q.BlockOut );   
        Run_PiReg_new( gyro->pi_quad_2, -gyro->pi_quad_1[0] );
    }

   // --- 9 -------- Коррекция амплитуд -----
    if ( gyro->pi_ampl_2[0] > (float) 9.9 ) gyro->pi_ampl_2[0] = (float) 9.9;
    if ( gyro->pi_ampl_2[0] < (float)-9.9 ) gyro->pi_ampl_2[0] = (float)-9.9;
    if ( gyro->pi_corr_2[0] > (float) 9.9 ) gyro->pi_corr_2[0] = (float) 9.9;
    if ( gyro->pi_corr_2[0] < (float)-9.9 ) gyro->pi_corr_2[0] = (float)-9.9;
    if ( gyro->pi_quad_2[0] > (float) 9.9 ) gyro->pi_quad_2[0] = (float) 9.9;
    if ( gyro->pi_quad_2[0] < (float)-9.9 ) gyro->pi_quad_2[0] = (float)-9.9;

   // --- 10-------- формирование гармонических сигналов ------------
    gyro->cos_exc  = cosf( gyro->rmnk.x_phi + gyro->ExcitationPhase   );
    gyro->cos_comp = cosf( gyro->rmnk.x_phi + gyro->CompensationPhase );
    gyro->sin_comp = sinf( gyro->rmnk.x_phi + gyro->CompensationPhase + gyro->PhiQPhase );

   // --- 11 ----------- Управляющие сигналы --------------------------
    gyro->GyroOutRaw = gyro->k_Omega * gyro->pi_corr_2[0];

    gyro->out_Ux     = - (gyro->pi_ampl_2[0] + gyro->c1 ) * gyro->cos_exc;
    gyro->out_Uy     = - (gyro->pi_corr_2[0] + gyro->c2 ) * gyro->cos_comp 
                       + (gyro->pi_quad_2[0] + gyro->c3 ) * gyro->sin_comp;


    Debug.TempFrequency = ALGO_FREQUENCY / (Debug.freq_incr * TWO_PI_INVERT * ALGO_FREQUENCY);
    if ( Debug.TempFrequency == 0 ) Debug.TempFrequency = 1;
    Calc_Phase( &gyro->Phase, Debug.TempFrequency, gyro->c2, -gyro->pi_corr_2[0] );

    

   // --- 12 ----------- Счетчик и флаг запуска 0.4 сек --------
    if ( ++gyro->AlgoCounter >= gyro->StartUpTime  ) gyro->isAlgoCounter = 2;
   // ******************************************
   // *** 13 **** Разгон(Старт) Гирика *********
   // ******************************************

    Calc_Period( &gyro->rmnk.ANodePeriod, gyro->fi_input_x[0] );
    if ( gyro->isAlgoCounter < 2 || gyro->DebugMode == 6 ) 
    {
        // --- Расчет простого периода колебаний системы
        // --- и перенос начального значения инкримента фазы в RMNK ---
        gyro->rmnk.delta_phi = TWO_PI / gyro->rmnk.ANodePeriod.Period;
        gyro->rmnk.delta_phi_m = gyro->rmnk.delta_phi;

        // -- ФАЗА (Задержка) АГ (перасчет для режима сканирование) ----
        if ( gyro->DebugMode == 6 ) 
        {
            gyro->Zn_exc.zn_curr = gyro->DefaultPeriod * gyro->ExcitationPhaseAG * TWO_PI_INVERT;
            Setup_Zn(&gyro->Zn_exc);
        }

        // ---- Скользящий буфера -----
    	memmove(&gyro->XBuffer[1], gyro->XBuffer, 100); 
    	gyro->XBuffer[0] = - gyro->fi_input_x[0];
        
        local_var    = Get_SplineValue( gyro->XBuffer, &gyro->Zn_exc);
        gyro->out_Ux = ( local_var >= (float)0.0 ) ? gyro->pi_ampl_2[0] : -gyro->pi_ampl_2[0];
    }


   // ******************************************
   // ***  14  *****   Отладка *****************
   // ******************************************
    
    if ( gyro->DebugMode == 0 ) 
    {   // ------ Debug -----
        if ( Debug.init_counter++ >= Debug.init_count ) Debug.isInited = 1;
        if ( Debug.isWork == 1 && Debug.isInited == 1 )
        {
            //Debug.param1[ Debug.work_counter ] = *Debug.debug_value;
            Debug.param1[ Debug.work_counter ] = gyro->fi_input_y[0];
            Debug.param2[ Debug.work_counter ] = gyro->rmnk.x_base_sin;
            //Debug.param2[ Debug.work_counter ] = gyro->fi_demod_c.BlockOut;//x_base_sin;

            if ( ++Debug.work_counter >= Debug.work_count )
            {
                Debug.isSleep      = 1;
                Debug.isWork       = 0;
                Debug.work_counter = 0;
            }
        }
    }

   // --- 15 ---------  Усреднение главных параметров ---------
    Calc_Means_Values_for_Correction( gyro );

   // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   // ---- 16 --------- Пошаговая система расчета коррекций ------------
    if ( ++gyro->CorrectionOrderCalc >= 50 ) gyro->CorrectionOrderCalc = 0;

    if ( gyro->CorrectionOrderCalc == 0 )
    {
        gyro->f1 = gyro->cf_Freq.BlockOut - corr_f0;
        gyro->f2 = gyro->f1*gyro->f1;     
        gyro->f3 = gyro->f1*gyro->f2;     
        gyro->f4 = gyro->f1*gyro->f3;     
        gyro->f5 = gyro->f1*gyro->f4;     
        gyro->f6 = gyro->f1*gyro->f5;     
    }
    if ( gyro->CorrectionOrderCalc == 1 )
    {
        gyro->q1 = gyro->cf_Quad.BlockOut - corr_q0;
        gyro->q2 = gyro->q1*gyro->q1;     
        gyro->q3 = gyro->q1*gyro->q2;     
        gyro->q4 = gyro->q1*gyro->q3;     
        gyro->q5 = gyro->q1*gyro->q4;     
        gyro->q6 = gyro->q1*gyro->q5;     
    }
    if ( gyro->CorrectionOrderCalc == 2 )
    {
        gyro->e1 = gyro->cf_Exci.BlockOut - corr_e0;
        gyro->e2 = gyro->e1*gyro->e1;     
        gyro->e3 = gyro->e1*gyro->e2;     
        gyro->e4 = gyro->e1*gyro->e3;     
        gyro->e5 = gyro->e1*gyro->e4;     
        gyro->e6 = gyro->e1*gyro->e5;     
    }
    if ( gyro->CorrectionOrderCalc == 3 )
    {
        gyro->t1 = gyro->cf_Temp.BlockOut - corr_t0;
        gyro->t2 = gyro->t1*gyro->t1;     
        gyro->t3 = gyro->t1*gyro->t2;     
        gyro->t4 = gyro->t1*gyro->t3;     
        gyro->t5 = gyro->t1*gyro->t4;     
        gyro->t6 = gyro->t1*gyro->t5;     
    }

    // **************************************************************************************
    if ( gyro->isMainSumInited == 0 ) return;

    // ------- Коррекции влияющие на СИСТЕМУ управления -------
    if ( gyro->CorrectionOrderCalc == 4 )
    { // ------ Полосовик -------
         cos_freq            = -(float)2.0 * cosf(gyro->cf_Freq.BlockOut * TWO_PI_DISCRET_TIME);// 
         gyro->fi_input_x[4] = cos_freq * gyro->fi_input_x[1];
         gyro->fi_input_y[4] = cos_freq * gyro->fi_input_y[1];
    }
    
    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 5 && gyro->isXPhase_corr == 1 )
    { 
        gyro->ExcitationPhase = corr_x_ph_a0 + corr_x_ph_a1 * gyro->f1 + corr_x_ph_a2 * gyro->f2;
    }
    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 6 && gyro->isYPhase_corr == 1 )
    { 
        gyro->CompensationPhase = gyro->Base_CompensationPhase + gyro->k_Omega * (corr_y_ph_a0 + corr_y_ph_a1*gyro->f1 + corr_y_ph_a2*gyro->f2);
    }


    // ------- ПОСТ Коррекции не влияющие на СИСТЕМУ управления -------
    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 7 && gyro->isAlfaE_f_corr == 1 )
    { 
        sc_argument  = corr_e_f0 + corr_e_f1*gyro->f1 + corr_e_f2*gyro->f2;

        gyro->corr_sin_alfa_e = sinf(sc_argument);
        gyro->corr_cos_alfa_e = cosf(sc_argument);
    }
    if ( gyro->CorrectionOrderCalc == 8 && gyro->isAlfaE_t_corr == 1 )
    { 
        sc_argument  = corr_e_t0 + corr_e_t1*gyro->t1 + corr_e_t2*gyro->t2;

        gyro->corr_sin_alfa_e = sinf(sc_argument);
        gyro->corr_cos_alfa_e = cosf(sc_argument);
    }
    if ( gyro->CorrectionOrderCalc == 9 && gyro->isAlfaQ_f_corr == 1 )
    { 
        sc_argument = corr_q_f0 + corr_q_f1*gyro->f1 + corr_q_f2*gyro->f2;

        gyro->corr_sin_alfa_q = sinf(sc_argument);
        gyro->corr_cos_alfa_q = cosf(sc_argument);
    }
    if ( gyro->CorrectionOrderCalc == 10 && gyro->isAlfaQ_t_corr == 1 )
    { 
        sc_argument = corr_q_t0 + corr_q_t1*gyro->t1 + corr_q_t2*gyro->t2;

        gyro->corr_sin_alfa_q = sinf(sc_argument);
        gyro->corr_cos_alfa_q = cosf(sc_argument);
    }

    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 11 && gyro->isStatic1_corr == 1 )
    {
        gyro->GyroOut_corr_st1_a =   corr_st1_f1*gyro->f1 + corr_st1_f2*gyro->f2 
                                   + corr_st1_f3*gyro->f3 + corr_st1_f4*gyro->f4 
                                   + corr_st1_f5*gyro->f5 + corr_st1_f6*gyro->f6;
    }
    if ( gyro->CorrectionOrderCalc == 12 && gyro->isStatic1_corr == 1 )
    {
        gyro->GyroOut_corr_st1_b =   corr_st1_e1*gyro->e1 + corr_st1_e2*gyro->e2 
                                   + corr_st1_e3*gyro->e3 + corr_st1_e4*gyro->e4 
                                   + corr_st1_e5*gyro->e5 + corr_st1_e6*gyro->e6;
    }
    if ( gyro->CorrectionOrderCalc == 13 && gyro->isStatic1_corr == 1 )
    {
        gyro->GyroOut_corr_st1_c =   corr_st1_q1*gyro->q1 + corr_st1_q2*gyro->q2 
                                   + corr_st1_q3*gyro->q3 + corr_st1_q4*gyro->q4 
                                   + corr_st1_q5*gyro->q5 + corr_st1_q6*gyro->q6;
    }
    if ( gyro->CorrectionOrderCalc == 14 && gyro->isStatic1_corr == 1 )
    {
        gyro->GyroOut_corr_st1 = corr_g0 + gyro->GyroOut_corr_st1_a + gyro->GyroOut_corr_st1_b + gyro->GyroOut_corr_st1_c 
                                + corr_st1_t1*gyro->t1 + corr_st1_t2*gyro->t2 
                                + corr_st1_t3*gyro->t3 + corr_st1_t4*gyro->t4 
                                + corr_st1_t5*gyro->t5 + corr_st1_t6*gyro->t6;
    }
    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 15 && gyro->isStatic2_corr == 1 )
    {
        gyro->GyroOut_corr_st2_a =   corr_st2_f1*gyro->f1 + corr_st2_f2*gyro->f2 
                                   + corr_st2_f3*gyro->f3 + corr_st2_f4*gyro->f4 
                                   + corr_st2_f5*gyro->f5 + corr_st2_f6*gyro->f6;
    }
    if ( gyro->CorrectionOrderCalc == 16 && gyro->isStatic2_corr == 1 )
    {
        gyro->GyroOut_corr_st2_b =   corr_st2_e1*gyro->e1 + corr_st2_e2*gyro->e2 
                                   + corr_st2_e3*gyro->e3 + corr_st2_e4*gyro->e4 
                                   + corr_st2_e5*gyro->e5 + corr_st2_e6*gyro->e6;
    }
    if ( gyro->CorrectionOrderCalc == 17 && gyro->isStatic2_corr == 1 )
    {
        gyro->GyroOut_corr_st2_c =   corr_st2_q1*gyro->q1 + corr_st2_q2*gyro->q2 
                                   + corr_st2_q3*gyro->q3 + corr_st2_q4*gyro->q4 
                                   + corr_st2_q5*gyro->q5 + corr_st2_q6*gyro->q6;
    }
    if ( gyro->CorrectionOrderCalc == 18 && gyro->isStatic2_corr == 1 )
    {
        gyro->GyroOut_corr_st2 = corr_g1 + gyro->GyroOut_corr_st2_a + gyro->GyroOut_corr_st2_b + gyro->GyroOut_corr_st2_c 
                                + corr_st2_t1*gyro->t1 + corr_st2_t2*gyro->t2 
                                + corr_st2_t3*gyro->t3 + corr_st2_t4*gyro->t4 
                                + corr_st2_t5*gyro->t5 + corr_st2_t6*gyro->t6;
    }
 

    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 19 && gyro->isDynamic_1_corr == 1 )
    {
        sc_argument = corr_mk1_offset + corr_mk1_f1 * gyro->f1 + corr_mk1_f2 * gyro->f2 + corr_mk1_f3 * gyro->f3
                                      + corr_mk1_e1 * gyro->e1 + corr_mk1_e2 * gyro->e2 + corr_mk1_e3 * gyro->e3
                                      + corr_mk1_q1 * gyro->q1 + corr_mk1_q2 * gyro->q2 + corr_mk1_q3 * gyro->q3
                                      + corr_mk1_t1 * gyro->t1 + corr_mk1_t2 * gyro->t2 + corr_mk1_t3 * gyro->t3;
        gyro->GyroOut_MK1 = corr_mk0 / sc_argument;
    }

    // **************************************************************************************
    if ( gyro->CorrectionOrderCalc == 20 && gyro->isDynamic_2_corr == 1 )
    {
        sc_argument = corr_mk2_offset + corr_mk2_f1 * gyro->f1 + corr_mk2_f2 * gyro->f2 + corr_mk2_f3 * gyro->f3
                                      + corr_mk2_e1 * gyro->e1 + corr_mk2_e2 * gyro->e2 + corr_mk2_e3 * gyro->e3
                                      + corr_mk2_q1 * gyro->q1 + corr_mk2_q2 * gyro->q2 + corr_mk2_q3 * gyro->q3
                                      + corr_mk2_t1 * gyro->t1 + corr_mk2_t2 * gyro->t2 + corr_mk2_t3 * gyro->t3;
        gyro->GyroOut_MK2 = corr_mk0 / sc_argument;
    }

    // ***** 17 ******** Выход ГИРОСКОПА ********
        // ------ Альфа Е Коррекция ------
        gyro->Excitation_alfa_e = gyro->pi_ampl_2[0] * gyro->corr_cos_alfa_e - gyro->GyroOutRaw   * gyro->corr_sin_alfa_e;
        gyro->GyroOut_alfa_e    = gyro->GyroOutRaw   * gyro->corr_cos_alfa_e + gyro->pi_ampl_2[0] * gyro->corr_sin_alfa_e;

        // ------ Альфа Q Коррекция ------
        gyro->Quadrature_alfa_q = gyro->pi_quad_2[0]   * gyro->corr_cos_alfa_q - gyro->GyroOut_alfa_e * gyro->corr_sin_alfa_q;
        gyro->GyroOut_alfa_q    = gyro->GyroOut_alfa_e * gyro->corr_cos_alfa_q + gyro->pi_quad_2[0]   * gyro->corr_sin_alfa_q;
    
        // ------ Остаточная коррекция (сюда должны входить все виды коррекции, статика1, динамика1, статика 2)------
        gyro->GyroOut_corr = ((gyro->GyroOut_alfa_q - gyro->GyroOut_corr_st1) * gyro->GyroOut_MK1 - gyro->GyroOut_corr_st2)  * gyro->GyroOut_MK2;
} 
示例#2
0
// *****************************************************************
// 
// *****************************************************************
void Run_Algo( TGyro *gyro ) 
{
    float cos_freq, sc_argument, local_var;
    Sint8 iCounter;

    // --- 1 ------ НАСРОЙКА ПРИБОРА -----
    if ( gyro->DebugMode != 0 ) 
    {
        SetupAlgo( gyro );

        gyro->ExcitationPhaseAG = gyro->Base_ExcitationPhaseAG +     gyro->c5;
        gyro->ExcitationPhase_x = gyro->Base_ExcitationPhase_x + 2.0*gyro->c6;
        gyro->ExcitationPhase_y = gyro->Base_ExcitationPhase_y + 2.0*gyro->c7;

        //if ( gyro->ExcitationPhaseAG < (float)0.0 ) gyro->ExcitationPhaseAG = (float)0.0;
        //if ( gyro->ExcitationPhase_x < (float)0.0 ) gyro->ExcitationPhase_x = (float)0.0;
        //if ( gyro->ExcitationPhase_y < (float)0.0 ) gyro->ExcitationPhase_x = (float)0.0;

		// ---- debug --- Для определения полосы пропускания -----
	    Debug.TempFrequency = ALGO_FREQUENCY / (Debug.freq_incr * TWO_PI_INVERT * ALGO_FREQUENCY);
	    if ( Debug.TempFrequency == 0 ) Debug.TempFrequency = 1;
	    Calc_Phase( &Debug.Phase, Debug.TempFrequency, gyro->c2, - gyro->pi_y_2[0] );
    }

   // --- 2 ------ ДАННЫЕ В UART -------
    Setup_UART_data( gyro );

   // --- 3 ------ Фильтрация входных сигналов ------
    Run_BandPassFiltration_new( gyro->fi_input_x, gyro->in_Ux );
    Run_BandPassFiltration_new( gyro->fi_input_y, gyro->in_Uy );

    // ---- Фаза между X, Y ----
    if ( gyro->DebugMode <= 4 ) 
        Calc_Phase( &gyro->Phase, gyro->rmnk_x.NCycles, gyro->fi_input_x[0], gyro->fi_input_y[0] );

    // --- 4 ------ RMNK ------ (R-рекуррентный M-метод N-наименьших K-квадратов)
    Run_Demod_RMNK( &gyro->rmnk_x, gyro->fi_input_x[0] );
    Run_Demod_RMNK( &gyro->rmnk_y, gyro->fi_input_y[0] );
    gyro->DeltaFrequency = gyro->rmnk_x.Frequency - gyro->rmnk_y.Frequency;

  // --- 7 ----- Регулирование амплитуды колебаний Канала Х,Y ----
    Run_PiReg_new( gyro->pi_x_1,  gyro->rmnk_x.fi_a[0] );  
    Run_PiReg_new( gyro->pi_x_2, -gyro->pi_x_1[0]  );
    
    Run_PiReg_new( gyro->pi_y_1,  gyro->rmnk_y.fi_a[0] );  
	Run_PiReg_new( gyro->pi_y_2, -gyro->pi_y_1[0]  );

   // --- 9 -------- Коррекция амплитуд -----
    if ( gyro->pi_x_2[0] > (float) 9.9 ) gyro->pi_x_2[0] = (float) 9.9;
    if ( gyro->pi_x_2[0] < (float)-9.9 ) gyro->pi_x_2[0] = (float)-9.9;
    if ( gyro->pi_y_2[0] > (float) 9.9 ) gyro->pi_y_2[0] = (float) 9.9;
    if ( gyro->pi_y_2[0] < (float)-9.9 ) gyro->pi_y_2[0] = (float)-9.9;

  // --- 8 ------ Если автогениратор отключился 
  // ---   ------ тогда переходим на компенсирование узла
  	
    //if ( gyro->fi_input_y[0] == 0 && gyro->fi_input_x[0] == 0 )
    //     gyro->NULL_value = 0.0;    
    //else gyro->NULL_value = atan2f ( gyro->fi_input_y[0], gyro->fi_input_x[0]) * RAD_TO_DEG; 
    
	if ( gyro->isAlgoCounter >= 3 )     
    { 
        //if ( gyro->isAlgoCounter >= 4 ) gyro->BaseAntinodePhase += ( 360.0 * DISCRET_TIME * 0.05 );
        if ( gyro->BaseAntinodePhase >= 360.0 ) gyro->BaseAntinodePhase -= 360;
        
        Run_LowPass_IRFiltr_1rstOrder ( gyro->fi_Phase, gyro->Phase.Phase_rad );
        
        Run_PiReg_new( gyro->pi_phi_1, - gyro->Phase.Phase_rad + 0.970*ONE_PI*sinf(gyro->BaseAntinodePhase*DEG_TO_RAD));
        Run_PiReg_new( gyro->pi_phi_2, - gyro->pi_phi_1[0] );
    }
        	
    
    
    
/*    
	if ( gyro->isAlgoCounter >= 3 ) 
    { 
        if ( gyro->isAlgoCounter >= 4 ) 
            gyro->BaseAntinodePhase += ( 360.0 * DISCRET_TIME * 0.05 );
    	if ( gyro->BaseAntinodePhase >= 360.0 ) gyro->BaseAntinodePhase -= 360;
    	
        
		if ( gyro->Phase.isNewValuePresent == 1 )
    	{
    	    if ( gyro->Phase.Phase_rad_old > 170 * DEG_TO_RAD && gyro->Phase.Phase_rad < -170 * DEG_TO_RAD)
    	    {
    	       //gyro->pi_phi_1[5] = (float)0.0;
    	       //gyro->pi_phi_1[6] = (float)0.0;
    	       //gyro->pi_phi_1[8] = (float)0.0;

    	       //gyro->pi_phi_2[5] = (float)0.0;
    	       //gyro->pi_phi_2[6] = (float)0.0;
    	       //gyro->pi_phi_2[8] = (float)0.0;
    	    }
    	    
	    	gyro->Phase.isNewValuePresent = 0;
	    	//Run_LowPass_IRFiltr_1rstOrder( gyro->fi_Phase, gyro->Phase.Phase_rad - gyro->BaseAntinodePhase*DEG_TO_RAD);
	    	
	    	Run_LowPass_IRFiltr_1rstOrder ( gyro->fi_Phase, gyro->Phase.Phase_rad - ONE_PI*sinf(gyro->BaseAntinodePhase*DEG_TO_RAD));
	        //Run_PiReg_new( gyro->pi_phi_1, -gyro->fi_Phase[0] );  
		    //Run_PiReg_new( gyro->pi_phi_2, -gyro->pi_phi_1[0] );
		    //gyro->pi_phi_2[0] = gyro->pi_phi_1[0];
    	}
    }
*/
   // --- 11 ----------- Управляющие сигналы --------------------------
    gyro->out_Ux = - (gyro->pi_x_2  [0] + gyro->c1 ) * cosf( gyro->rmnk_x.phi + gyro->ExcitationPhase_x )
                   -  gyro->pi_phi_2[0] * 1.0        * sinf( gyro->rmnk_x.phi + gyro->ExcitationPhase_x );
                   
    gyro->out_Uy = - (gyro->pi_y_2  [0] + gyro->c2 ) * cosf( gyro->rmnk_y.phi + gyro->ExcitationPhase_y )
                   +  gyro->pi_phi_2[0] * 1.0        * sinf( gyro->rmnk_y.phi + gyro->ExcitationPhase_y ); 

                
    //gyro->out_Uy = (float)0.0;
    gyro->GyroOut    = (float)0.5 * (gyro->pi_x_2[0] - gyro->pi_y_2[0]);
    gyro->Quadrature = (float)0.5 * (gyro->pi_x_2[0] + gyro->pi_y_2[0]);

    // ----- Если сканироваение Стартовой фазы, зануляю УЗЕЛ ------
    if ( gyro->DebugMode == 5 ) gyro->out_Uy = 0;
    if ( gyro->DebugMode == 6 ) gyro->out_Uy = 0;
    if ( gyro->DebugMode == 7 ) gyro->out_Ux = 0;

    // --- 12 ----------- Счетчик и флаг запуска 0.4 сек --------
    gyro->AlgoCounter++;
    if ( gyro->AlgoCounter >=  gyro->StartUpTime  ) gyro->isAlgoCounter = 2;
    if ( gyro->AlgoCounter >=  50000              ) gyro->isAlgoCounter = 3;
    if ( gyro->AlgoCounter >= 500000              ) gyro->isAlgoCounter = 4;
    
   // ******************************************
   // *** 13 **** Разгон(Старт) Гирика *********
   // ******************************************
    Calc_Period( &gyro->rmnk_x.ANodePeriod, gyro->fi_input_x[0] );
    if ( gyro->isAlgoCounter < 2 || gyro->DebugMode == 5 ) 
    {
        // --- Расчет простого периода колебаний системы
        // --- и перенос начального значения инкримента фазы в RMNK ---
        gyro->rmnk_x.delta_phi = TWO_PI / gyro->rmnk_x.ANodePeriod.Period;
        gyro->rmnk_y.delta_phi = gyro->rmnk_x.delta_phi;

        // -- ФАЗА (Задержка) АГ (перасчет для режима сканирование) ----
        if ( gyro->DebugMode == 5 ) 
        {
            gyro->Zn_exc.zn_curr = gyro->DefaultPeriod * gyro->ExcitationPhaseAG * TWO_PI_INVERT;
            Setup_Zn(&gyro->Zn_exc);
        }

        // ---- Скользящий буфера -----
    	memmove(&gyro->XBuffer[1], gyro->XBuffer, 100); 
    	gyro->XBuffer[0] = - gyro->fi_input_x[0];
        
        local_var    = Get_SplineValue( gyro->XBuffer, &gyro->Zn_exc);
        gyro->out_Ux = ( local_var >= (float)0.0 ) ? gyro->pi_x_2[0] : -gyro->pi_x_2[0];
        gyro->out_Uy = (float)0.0;
    }


   // ******************************************
   // ***  14  *****   Отладка *****************
   // ******************************************
    if ( gyro->DebugMode == 0 ) 
    {   // ------ Debug -----
        if ( Debug.init_counter++ >= Debug.init_count ) Debug.isInited = 1;
        if ( Debug.isWork == 1 && Debug.isInited == 1 )
        {
            Debug.param1[ Debug.work_counter ] = *Debug.debug_value;

            if ( ++Debug.work_counter >= Debug.work_count )
            {
                Debug.isSleep      = 1;
                Debug.isWork       = 0;
                Debug.work_counter = 0;
            }
        }
    }
}