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