// *****************************************************************
//
// *****************************************************************
void Run_Demod_RMNK( TRMNK *prmnk, float inX )
{
float HQx;
float xa2, xb2;
float k = 10000.0;
// ----- 5.0 * TWO_PI * DISCRET_TIME = (float)0.0006283185307179586476925286766559
prmnk->XPhaseIncriment = prmnk->delta_phi + (float)0.2 * TWO_PI_DISCRET_TIME * prmnk->pi_pll_x2.BlockOut + prmnk->c4;
prmnk->XFrequency = prmnk->XPhaseIncriment * TWO_PI_INVERT_ALGO_FREQUENCY;
prmnk->x_phi = prmnk->x_phi + prmnk->XPhaseIncriment;
if ( prmnk->x_phi >= TWO_PI ) prmnk->x_phi = prmnk->x_phi - TWO_PI;
prmnk->x_base_sin = sinf( prmnk->x_phi );
prmnk->x_base_cos = cosf( prmnk->x_phi );
// --------------------------------------------
HQx = prmnk->Qx[0] + prmnk->x_base_sin*prmnk->Qx[1] + prmnk->x_base_cos*prmnk->Qx[2] - inX;
prmnk->Qx[0] = prmnk->Qx[0] - prmnk->MjuX[0] * HQx;
prmnk->Qx[1] = prmnk->Qx[1] - prmnk->MjuX[1] * prmnk->x_base_sin * HQx;
prmnk->Qx[2] = prmnk->Qx[2] - prmnk->MjuX[2] * prmnk->x_base_cos * HQx;
// ------ фильтрование КВАДРАТа ПУЧНОСТИ -----
Run_Z2Coef( &prmnk->fi_Xa, prmnk->Qx[1]*prmnk->Qx[1] + prmnk->Qx[2]*prmnk->Qx[2] );
// ------ фильтрованный КВАДРАТ ПУЧНОСТИ -------
prmnk->XAmpl = prmnk->fi_Xa.BlockOut;
// ------ фильтрация -------
Run_Z2Coef( &prmnk->fi_Xb, prmnk->Qx[2] );
if ( prmnk->ANodePeriod.isNewValuePresent == 1 )
{
prmnk->ANodePeriod.isNewValuePresent = 0;
Run_Z2Coef( &prmnk->fi_Xc, prmnk->ANodePeriod.Period );
//prmnk->delta_phi = TWO_PI / prmnk->fi_Xc.BlockOut;
//prmnk->delta_phi = TWO_PI / prmnk->ANodePeriod.Period;
}
// ------ Регулирование отфильтрованного параметра prmnk->Qx[2] -------
Run_PiReg( &prmnk->pi_pll_x1, prmnk->fi_Xb.BlockOut );
Run_PiReg( &prmnk->pi_pll_x2, -prmnk->pi_pll_x1.BlockOut );
//prmnk->pi_pll_x2.BlockOut = prmnk->pi_pll_x1.BlockOut;
}
// *****************************************************************
//
// *****************************************************************
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;
if ( gyro->ExcitationPhaseAG < (float)0.0 ) gyro->ExcitationPhaseAG = (float)0.0;
if ( gyro->ExcitationPhase < (float)0.0 ) gyro->ExcitationPhase = (float)0.0;
// ----- Если сканироваение Стартовой фазы, зануляю УЗЕЛ ------
if ( gyro->DebugMode == 6 ) gyro->in_Uy = 0;
}
// --- 2 ------ ДАННЫЕ В UART -------
Setup_UART_data( gyro );
// --- 3 ------ Фильтрация входных сигналов ------
Input_Filtration_short( &gyro->fi_input_x , gyro->in_Ux );
Input_Filtration_short( &gyro->fi_input_y , gyro->in_Uy );
// --- 4 ------ RMNK ------ (R-рекуррентный M-метод N-наименьших K-квадратов)
//**********************
Run_Demod_RMNK( &gyro->rmnk, gyro->DebugCoef*gyro->fi_input_x.BlockOut );
if ( gyro->DebugCoef < 0 )
gyro->rmnk.x_phi_180 = gyro->rmnk.x_phi + ONE_PI;
else gyro->rmnk.x_phi_180 = gyro->rmnk.x_phi;
//**********************
// --- 5 ------ Амплитуда КОЛебаний канала У -----
if ( gyro->Y_minVal > gyro->fi_input_y.BlockOut ) gyro->Y_minVal = gyro->fi_input_y.BlockOut;
if ( gyro->Y_maxVal < gyro->fi_input_y.BlockOut ) gyro->Y_maxVal = gyro->fi_input_y.BlockOut;
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.BlockOut * sinf( gyro->rmnk.x_phi_180 ) );
Run_Demod( &gyro->fi_demod_q, - gyro->fi_input_y.BlockOut * cosf( gyro->rmnk.x_phi_180 ) );//
// --- 7 ----- Регулирование колебаний амплитуды пучности ----
Run_PiReg( &gyro->pi_am_1, gyro->rmnk.XAmpl );
Run_PiReg( &gyro->pi_am_2, -gyro->pi_am_1.BlockOut );
// --- 8 ------ Если автогениратор отключился
// --- ------- тогда переходим на компенсирование узла
if ( gyro->isAlgoCounter >= 2 )
{
Run_PiReg( &gyro->pi_om_1, gyro->fi_demod_c.BlockOut );
Run_PiReg( &gyro->pi_om_2, -gyro->pi_om_1 .BlockOut );
Run_PiReg( &gyro->pi_si_1, gyro->fi_demod_q.BlockOut );
Run_PiReg( &gyro->pi_si_2, -gyro->pi_si_1 .BlockOut );
}
// --- 9 -------- Коррекция амплитуд -----
if ( gyro->pi_am_2.BlockOut > (float) 9.9 ) gyro->pi_am_2.BlockOut = (float) 9.9;
if ( gyro->pi_am_2.BlockOut < (float)-9.9 ) gyro->pi_am_2.BlockOut = (float)-9.9;
if ( gyro->pi_om_2.BlockOut > (float) 9.9 ) gyro->pi_om_2.BlockOut = (float) 9.9;
if ( gyro->pi_om_2.BlockOut < (float)-9.9 ) gyro->pi_om_2.BlockOut = (float)-9.9;
if ( gyro->pi_si_2.BlockOut > (float) 9.9 ) gyro->pi_si_2.BlockOut = (float) 9.9;
if ( gyro->pi_si_2.BlockOut < (float)-9.9 ) gyro->pi_si_2.BlockOut = (float)-9.9;
// --- 10-------- формирование гармонических сигналов ------------
gyro->cos_exc = cosf( gyro->rmnk.x_phi_180 + gyro->ExcitationPhase );
gyro->cos_comp = cosf( gyro->rmnk.x_phi_180 + gyro->CompensationPhase );
gyro->sin_comp = sinf( gyro->rmnk.x_phi_180 + gyro->CompensationPhase + gyro->PhiQPhase );
// --- 11 ----------- Управляющие сигналы --------------------------
gyro->GyroOutRaw = gyro->k_Omega * gyro->pi_om_2.BlockOut;
gyro->DebugCounter = gyro->DebugCounter + DISCRET_TIME;
if ( gyro->DebugCounter >= (float)60.0 )
{
gyro->DebugCounter1 = gyro->DebugCounter1 + (float)1.0;
if ( gyro->DebugCounter1 >= (float)10.0 )
{
gyro->DebugCoef = gyro->DebugCoef * (float) -1.0;
gyro->NULL_value = gyro->DebugCoef;
gyro->DebugCounter1 = (float)0.0;
}
gyro->DebugCounter = (float)0.0;
}
gyro->out_Ux = - (gyro->pi_am_2.BlockOut + gyro->c1) * gyro->cos_exc;
gyro->out_Uy = - (gyro->pi_om_2.BlockOut + gyro->c2) * gyro->cos_comp
+ (gyro->pi_si_2.BlockOut + gyro->c3) * gyro->sin_comp;
// --- 12 ----------- Счетчик и флаг запуска 0.4 сек --------
if ( ++gyro->AlgoCounter >= gyro->StartUpTime ) gyro->isAlgoCounter = 2;
// ******************************************
// *** 13 **** Разгон(Старт) Гирика *********
// ******************************************
Calc_Period( &gyro->rmnk.ANodePeriod, gyro->fi_input_x.BlockOut );
if ( gyro->isAlgoCounter < 2 || gyro->DebugMode == 6 )
{
// --- Расчет простого периода колебаний системы
// --- и перенос начального значения инкримента фазы в RMNK ---
gyro->rmnk.delta_phi = TWO_PI / gyro->rmnk.ANodePeriod.Period;
// -- ФАЗА (Задержка) АГ (перасчет для режима сканирование) ----
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.BlockOut;
local_var = Get_SplineValue( gyro->XBuffer, &gyro->Zn_exc);
gyro->out_Ux = ( local_var >= (float)0.0 ) ? gyro->pi_am_2.BlockOut : -gyro->pi_am_2.BlockOut;
}
// ******************************************
// *** 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;
}
}
}
*/
// --- 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.b[1] = cos_freq * gyro->filter_invertValue_x;
gyro->fi_input_y.b[1] = cos_freq * gyro->filter_invertValue_y;
}
// **************************************************************************************
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 == 14 && gyro->isDynamic_corr == 1 )
{
sc_argument = 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;
}
// ***** 17 ******** Выход ГИРОСКОПА ********
// ------ Альфа Е Коррекция ------
gyro->Excitation_alfa_e = gyro->pi_am_2.BlockOut * 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_am_2.BlockOut * gyro->corr_sin_alfa_e;
// ------ Альфа Q Коррекция ------
gyro->Quadrature_alfa_q = gyro->pi_si_2.BlockOut * 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_si_2.BlockOut * 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;
}
//*****************************************************************
//
//*****************************************************************
void Run_Algo( TGyro *gyro )
{
// ------ ДАННЫЕ В UART -------
Setup_UART_data( gyro );
// ----- Фильтрация входных сигнаов ------
Input_Filtration_short( &gyro->fi_input_x , gyro->in_Ux );
Input_Filtration_short( &gyro->fi_input_y , gyro->in_Uy );
// ------ RMNK -----
Run_Demod_RMNK( &gyro->rmnk, gyro->in_Ux, gyro->fi_input_x.BlockOut, gyro->fi_input_y.BlockOut );
// ------- Демодуляция и Маштабирование ---
Run_Demod( &gyro->fi_demod_c, gyro->fi_input_y.BlockOut * gyro->rmnk.x_base_sin );
Run_Demod( &gyro->fi_demod_q, gyro->fi_input_y.BlockOut * gyro->rmnk.x_base_cos );
// ----- Фильтрация параметра ---
Run_Z3Coef( &gyro->fi_AmplX, gyro->rmnk.XAmpl );
// ----- Регулирование ----
Run_PiReg( &gyro->pi_am_1, gyro->fi_AmplX .BlockOut ); Run_PiReg( &gyro->pi_am_2, -gyro->pi_am_1.BlockOut );
//Run_PiReg( &gyro->pi_am_1, gyro->rmnk.XAmpl ); Run_PiReg( &gyro->pi_am_2, -gyro->pi_am_1.BlockOut );
Run_PiReg( &gyro->pi_om_1, gyro->fi_demod_c.BlockOut ); Run_PiReg( &gyro->pi_om_2, -gyro->pi_om_1.BlockOut );
Run_PiReg( &gyro->pi_si_1, gyro->fi_demod_q.BlockOut ); Run_PiReg( &gyro->pi_si_2, -gyro->pi_si_1.BlockOut );
// -------- Коррекция амплитуд -----
if ( gyro->pi_am_2.BlockOut > (float) 9.5 ) gyro->pi_am_2.BlockOut = (float) 9.5;
if ( gyro->pi_am_2.BlockOut < (float)-9.5 ) gyro->pi_am_2.BlockOut = (float)-9.5;
if ( gyro->pi_om_2.BlockOut > (float) 9.5 ) gyro->pi_om_2.BlockOut = (float) 9.5;
if ( gyro->pi_om_2.BlockOut < (float)-9.5 ) gyro->pi_om_2.BlockOut = (float)-9.5;
if ( gyro->pi_si_2.BlockOut > (float) 9.5 ) gyro->pi_si_2.BlockOut = (float) 9.5;
if ( gyro->pi_si_2.BlockOut < (float)-9.5 ) gyro->pi_si_2.BlockOut = (float)-9.5;
// --------
//gyro->X_Phase_Algo = gyro->ExcitationPhase + gyro->K_XPhase * gyro->pi_XPhase.BlockOut;
//gyro->Y_Phase_Algo = gyro->CompensationPhase + gyro->K_XPhase * gyro->pi_XPhase.BlockOut;
gyro->X_Phase_Algo = gyro->ExcitationPhase + gyro->BPFPhase.Phase_rad;// + gyro->K_XPhase * gyro->pi_XPhase.BlockOut;
gyro->Y_Phase_Algo = gyro->CompensationPhase + gyro->BPFPhase.Phase_rad;
// -------- Управляющие сигналы ------------
gyro->sin_exc = - sinf( gyro->rmnk.x_phi + gyro->X_Phase_Algo );
gyro->sin_comp = - sinf( gyro->rmnk.x_phi + gyro->Y_Phase_Algo );
gyro->cos_comp = - cosf( gyro->rmnk.x_phi + gyro->Y_Phase_Algo );
gyro->GyroOutRaw = k_omega * gyro->pi_om_2.BlockOut;
gyro->GyroOut = gyro->GyroOutRaw;
gyro->out_Ux = gyro->pi_am_2.BlockOut * gyro->sin_exc;
gyro->out_Uy = gyro->pi_si_2.BlockOut * gyro->sin_comp;
gyro->out_Uy -= gyro->pi_om_2.BlockOut * gyro->cos_comp;
gyro->out_Uy *= gyro->isCompensationPresent;
// ------ Коеф передачи, только в конце, потомучто в начале схемы он добавляет шумы ---------
if ( gyro->isKFilter_CORR == 1 ) gyro->out_Uy *= gyro->rmnk.KFilter;
Calc_Phase ( &gyro->XPhase , gyro->rmnk.XPeriod, gyro->rmnk.XPeriod_invert, 0, gyro->sin_exc, gyro->in_Ux );
Calc_Phase ( &gyro->BPFPhase, gyro->rmnk.XPeriod, gyro->rmnk.XPeriod_invert, 0, gyro->in_Ux , gyro->fi_input_x.BlockOut );
if ( gyro->XPhase.isNewValuePresent == 1 && gyro->isAlgoCounter >= 4 && gyro->isPhaseCorrectionPresent == 1)
{
gyro->XPhase.isNewValuePresent = 0;
gyro->pi_XPhase.BlockInput = gyro->XPhase.Phase_rad - gyro->ExcitationPhase;
Run_PiReg( &gyro->pi_XPhase, gyro->pi_XPhase.BlockInput );
}
//----------- Разгон резонатора --------------------------
gyro->AlgoCounter++;
if ( gyro->AlgoCounter >= 20000 ) gyro->isAlgoCounter = 2;
if ( gyro->AlgoCounter >= 30000 ) gyro->isAlgoCounter = 3;
if ( gyro->AlgoCounter >= 50000 ) gyro->isAlgoCounter = 4;
if ( gyro->AlgoCounter >= 500000 ) gyro->isAlgoCounter = 5;
/*
if ( gyro->isAlgoCounter == 2 && gyro->isIntegCorrected == 0)
{
gyro->isIntegCorrected = 1;
gyro->pi_am_1.Integral.a[0] = gyro->pi_am_1.Integral.a[0] * (float)20.0;
gyro->pi_om_1.Integral.a[0] = gyro->pi_om_1.Integral.a[0] * (float)20.0;
gyro->pi_si_1.Integral.a[0] = gyro->pi_si_1.Integral.a[0] * (float)20.0;
gyro->pi_am_1.Integral.a[1] = gyro->pi_am_1.Integral.a[0];
gyro->pi_om_1.Integral.a[1] = gyro->pi_om_1.Integral.a[0];
gyro->pi_si_1.Integral.a[1] = gyro->pi_si_1.Integral.a[0];
}
if ( gyro->isAlgoCounter >= 5 && gyro->isIntegCorrected == 1)
{
gyro->isIntegCorrected = 2;
gyro->pi_am_1.Integral.a[0] = gyro->pi_am_1.Integral.a[0] * (float)0.05;
gyro->pi_om_1.Integral.a[0] = gyro->pi_om_1.Integral.a[0] * (float)0.05;
gyro->pi_si_1.Integral.a[0] = gyro->pi_si_1.Integral.a[0] * (float)0.05;
gyro->pi_am_1.Integral.a[1] = gyro->pi_am_1.Integral.a[0];
gyro->pi_om_1.Integral.a[1] = gyro->pi_om_1.Integral.a[0];
gyro->pi_si_1.Integral.a[1] = gyro->pi_si_1.Integral.a[0];
}
*/
// ------ ОТЛАДКА -----
if ( gyro->DebugMode != 0 ) SetupAlgo( gyro );
// ------ Debug -----
if ( Debug.init_counter++ >= Debug.init_count ) Debug.isInited = 1;
if ( Debug.isWork == 1 && Debug.isInited == 1 )
{
//Debug.param1[ Debug.work_counter ] = gyro->rmnk.XAmpl;
//Debug.param2[ Debug.work_counter ] = gyro->fi_demod_c.BlockOut; //gyro->fi_input_x.BlockOut;//gyro->RMNK.phi;//mod_s2;//gyro->out_Uy;
//Debug.param3[ Debug.work_counter ] = gyro->fi_demod_q.BlockOut;//;//gyro->RMNK.phi;//mod_s2;//gyro->out_Uy;
Debug.param1[ Debug.work_counter ] = gyro->pi_am_2.BlockOut;
Debug.param2[ Debug.work_counter ] = gyro->pi_om_2.BlockOut; //gyro->fi_input_x.BlockOut;//gyro->RMNK.phi;//mod_s2;//gyro->out_Uy;
Debug.param3[ Debug.work_counter ] = gyro->pi_si_2.BlockOut;//;//gyro->RMNK.phi;//mod_s2;//gyro->out_Uy;
if ( ++Debug.work_counter >= Debug.work_count )
{
Debug.isSleep = 1;
Debug.isWork = 0;
Debug.work_counter = 0;
}
}
}
// *****************************************************************
//
// *****************************************************************
void Run_Demod_RMNK( TRMNK *prmnk, float inX )
{
float HQx;
float xa2, xb2;
float k = 10000.0;
// ----- 5.0 * TWO_PI * DISCRET_TIME = (float)0.0006283185307179586476925286766559
/*
prmnk->XPhaseIncriment = k * prmnk->delta_phi + prmnk->pi_pll_x1.BlockOut + prmnk->c4;
prmnk->XPhaseIncriment /= k;
prmnk->XFrequency = prmnk->XPhaseIncriment * TWO_PI_INVERT_ALGO_FREQUENCY;
prmnk->x_phi = prmnk->x_phi + prmnk->XPhaseIncriment;
if ( prmnk->x_phi >= TWO_PI ) prmnk->x_phi = prmnk->x_phi - TWO_PI;
*/
prmnk->XPhaseIncriment = prmnk->delta_phi + (float)0.0006283185307179586476925286766559 * prmnk->pi_pll_x2.BlockOut + prmnk->c4;
prmnk->XFrequency = prmnk->XPhaseIncriment * TWO_PI_INVERT_ALGO_FREQUENCY;
prmnk->x_phi = prmnk->x_phi + prmnk->XPhaseIncriment;
if ( prmnk->x_phi >= TWO_PI ) prmnk->x_phi = prmnk->x_phi - TWO_PI;
prmnk->x_base_sin = sinf( prmnk->x_phi );
prmnk->x_base_cos = cosf( prmnk->x_phi );
// --------------------------------------------
HQx = prmnk->Qx[0] + prmnk->x_base_sin*prmnk->Qx[1] + prmnk->x_base_cos*prmnk->Qx[2] - inX;
prmnk->Qx[0] = prmnk->Qx[0] - prmnk->MjuX[0] * HQx;
prmnk->Qx[1] = prmnk->Qx[1] - prmnk->MjuX[1] * prmnk->x_base_sin * HQx;
prmnk->Qx[2] = prmnk->Qx[2] - prmnk->MjuX[2] * prmnk->x_base_cos * HQx;
// ------ фильтрование КВАДРАТа ПУЧНОСТИ -----
Run_Z2Coef( &prmnk->fi_Xa, prmnk->Qx[1]*prmnk->Qx[1] + prmnk->Qx[2]*prmnk->Qx[2] );
// ------ фильтрованный КВАДРАТ ПУЧНОСТИ -------
prmnk->XAmpl = prmnk->fi_Xa.BlockOut;
// ------ фильтрация -------
Run_Z2Coef( &prmnk->fi_Xb, prmnk->Qx[2] );
// ------ Регулирование отфильтрованного параметра prmnk->Qx[2] -------
Run_PiReg( &prmnk->pi_pll_x1, prmnk->fi_Xb.BlockOut );
Run_PiReg( &prmnk->pi_pll_x2, -prmnk->pi_pll_x1.BlockOut );
/*
prmnk->XPhaseIncriment = prmnk->delta_phi + (float)0.0006283185307179586476925286766559 * (prmnk->pi_pll_x2.BlockOut + prmnk->val) + prmnk->c4;
//prmnk->XPhaseIncriment = k * prmnk->delta_phi + prmnk->pi_pll_x1.BlockOut + prmnk->c4;
//prmnk->XPhaseIncriment /= k;
prmnk->XFrequency = prmnk->XPhaseIncriment * TWO_PI_INVERT_ALGO_FREQUENCY;
prmnk->x_phi = prmnk->x_phi + prmnk->XPhaseIncriment;
if ( prmnk->x_phi >= TWO_PI ) prmnk->x_phi = prmnk->x_phi - TWO_PI;
prmnk->x_base_sin = sinf( prmnk->x_phi );
prmnk->x_base_cos = cosf( prmnk->x_phi );
// --------------------------------------------
HQx = prmnk->Qx[0] + prmnk->x_base_sin*prmnk->Qx[1] + prmnk->x_base_cos*prmnk->Qx[2] - inX;
prmnk->Qx[0] = prmnk->Qx[0] - prmnk->MjuX[0] * HQx;
prmnk->Qx[1] = prmnk->Qx[1] - prmnk->MjuX[1] * prmnk->x_base_sin * HQx;
prmnk->Qx[2] = prmnk->Qx[2] - prmnk->MjuX[2] * prmnk->x_base_cos * HQx;
// ------ фильтрование КВАДРАТа ПУЧНОСТИ -----
Run_Z2Coef( &prmnk->fi_Xa, prmnk->Qx[1]*prmnk->Qx[1] + prmnk->Qx[2]*prmnk->Qx[2] );
// ------ фильтрованный КВАДРАТ ПУЧНОСТИ -------
prmnk->XAmpl = prmnk->fi_Xa.BlockOut;
// ------ фильтрация -------
Run_Z2Coef( &prmnk->fi_Xb, prmnk->Qx[2] );
// ------ Регулирование отфильтрованного параметра prmnk->Qx[2] -------
Run_PiReg( &prmnk->pi_pll_x1, prmnk->fi_Xb.BlockOut );
//Run_PiReg( &prmnk->pi_pll_x2, -prmnk->pi_pll_x1.BlockOut );
prmnk->pi_pll_x2.BlockOut = prmnk->pi_pll_x1.BlockOut;
prmnk->Summ += prmnk->pi_pll_x1.BlockOut;
if ( ++prmnk->Counter >= 500 )
{
prmnk->Summ *= (float)0.002;
if ( prmnk->Summ < -5e-6 ) prmnk->val -= 3e-6;
if ( prmnk->Summ > 5e-6 ) prmnk->val += 3e-6;
prmnk->Counter = 0;
prmnk->Summ = (float)0.0;
}
*/
}
//*****************************************************************
//
//*****************************************************************
void Run_Demod_RMNK( TRMNK *prmnk, float inX_raw, float inX, float inY )
{
float HQx_raw, HQx, HQy, HQyP;
float MjuX_raw[3], MjuX[3], MjuY[3], MjuYP[3];
float XPhaseIncriment, YPhaseIncriment;
MjuX_raw[0] = (float)1e-5;
MjuX_raw[1] = (float)0.005;
MjuX_raw[2] = (float)0.005;
MjuX[0] = (float)1e-5;
MjuX[1] = (float)0.001;
MjuX[2] = (float)0.001;
MjuY[0] = (float)1e-4;
MjuY[1] = (float)0.003;
MjuY[2] = (float)0.003;
//MjuYP[0] = (float)0.0001;
//MjuYP[1] = (float)0.001;
//MjuYP[2] = (float)0.001;
XPhaseIncriment = prmnk->delta_phi + prmnk->pi_pll_x2.BlockOut;
prmnk->XPeriod_invert = XPhaseIncriment * TWO_PI_INVERT;
prmnk->XPeriod = TWO_PI / XPhaseIncriment;
prmnk->XFrequency = prmnk->XPeriod_invert * ALGO_FREQUENCY;
prmnk->x_phi = prmnk->x_phi + XPhaseIncriment;
if ( prmnk->x_phi >= TWO_PI ) prmnk->x_phi -= TWO_PI;
prmnk->x_base_sin = sinf(prmnk->x_phi);
prmnk->x_base_cos = cosf(prmnk->x_phi);
/*
YPhaseIncriment = prmnk->delta_phi + prmnk->pi_pll_y.BlockOut;
prmnk->YPeriod_invert = YPhaseIncriment * TWO_PI_INVERT;
prmnk->YPeriod = TWO_PI / YPhaseIncriment;
prmnk->YFrequency = prmnk->YPeriod_invert * ALGO_FREQUENCY;
prmnk->y_phi = prmnk->y_phi + YPhaseIncriment;
if ( prmnk->y_phi >= TWO_PI ) prmnk->y_phi -= TWO_PI;
prmnk->y_base_sin = sinf(prmnk->y_phi);
prmnk->y_base_cos = cosf(prmnk->y_phi);
*/
// *****************************************************************
// ****** Оценка амплитуды сигнала перед Полосовым фильтром *****
// *****************************************************************
// ---------- Только для Оценки Коефиициента К --------
HQx_raw = prmnk->Qx_raw[0] + prmnk->x_base_sin*prmnk->Qx_raw[1] + prmnk->x_base_cos*prmnk->Qx_raw[2] - inX_raw;
prmnk->Qx_raw[0] = prmnk->Qx_raw[0] - MjuX_raw[0] * HQx_raw;
prmnk->Qx_raw[1] = prmnk->Qx_raw[1] - MjuX_raw[1] * prmnk->x_base_sin * HQx_raw;
prmnk->Qx_raw[2] = prmnk->Qx_raw[2] - MjuX_raw[2] * prmnk->x_base_cos * HQx_raw;
prmnk->XAmpl = sqrtf(prmnk->Qx_raw[1]*prmnk->Qx_raw[1] + prmnk->Qx_raw[2]*prmnk->Qx_raw[2]);
// *****************************************************************
// ****** Оценка фильтрованных параметров Канала X **************
// *****************************************************************
// -------- СТРОЮ ПЛЛ по нему Возбуждение и компенсация ---------
HQx = prmnk->Qx[0] + prmnk->x_base_sin*prmnk->Qx[1] + prmnk->x_base_cos*prmnk->Qx[2] - inX;
prmnk->Qx[0] = prmnk->Qx[0] - MjuX[0] * HQx;
prmnk->Qx[1] = prmnk->Qx[1] - MjuX[1] * prmnk->x_base_sin * HQx;
prmnk->Qx[2] = prmnk->Qx[2] - MjuX[2] * prmnk->x_base_cos * HQx;
// ----- Построение ФАпча X -------
Run_PiReg( &prmnk->pi_pll_x1, prmnk->Qx[2] );
Run_PiReg( &prmnk->pi_pll_x2, -prmnk->pi_pll_x1.BlockOut );
// ---- Коефициент передачи полосового фильтра ------
//prmnk->KFilter = - prmnk->base_XAmpl / prmnk->Qx[1];
prmnk->KFilter = - prmnk->XAmpl / prmnk->Qx[1];
prmnk->XMean = prmnk->Qx[0];
//prmnk->XAmpl = - prmnk->Qx[1];
// ******************************************************************
// ****** Оценка фильтрованных параметров Канала Y **************
// ******************************************************************
HQy = prmnk->Qy[0] + prmnk->x_base_sin*prmnk->Qy[1] + prmnk->x_base_cos*prmnk->Qy[2] - inY;
prmnk->Qy[0] = prmnk->Qy[0] - MjuY[0] * HQy;
prmnk->Qy[1] = prmnk->Qy[1] - MjuY[1] * prmnk->x_base_sin * HQy;
prmnk->Qy[2] = prmnk->Qy[2] - MjuY[2] * prmnk->x_base_cos * HQy;
prmnk->YMean = prmnk->Qy[0];
//prmnk->YAmpl = sqrtf( prmnk->Qy[1]*prmnk->Qy[1] + prmnk->Qy[2]*prmnk->Qy[2] );
//prmnk->CAmpl = -prmnk->Qy[1];
//prmnk->QAmpl = -prmnk->Qy[2];
prmnk->YAmpl = sqrtf( prmnk->Qy[1]*prmnk->Qy[1] + prmnk->Qy[2]*prmnk->Qy[2] ) * prmnk->KFilter;
//prmnk->CAmpl = -prmnk->Qy[1] * prmnk->KFilter;
//prmnk->QAmpl = -prmnk->Qy[2] * prmnk->KFilter;
// ******************************************************************
// ****** Оценка фильтрованных параметров Канала Y **************
// ******************************************************************
/*
HQyP = prmnk->QyP[0] + prmnk->y_base_sin*prmnk->QyP[1] + prmnk->y_base_cos*prmnk->QyP[2] - inY;
prmnk->QyP[0] = prmnk->QyP[0] - MjuYP[0] * HQyP;
prmnk->QyP[1] = prmnk->QyP[1] - MjuYP[1] * prmnk->y_base_sin * HQyP;
prmnk->QyP[2] = prmnk->QyP[2] - MjuYP[2] * prmnk->y_base_cos * HQyP;
// ----- Построение ФАпча X -------
Run_PiReg( &prmnk->pi_pll_y, prmnk->QyP[2] );
prmnk->YMean = prmnk->Qy[0];
prmnk->YAmpl = sqrtf( prmnk->Qy[1]*prmnk->Qy[1] + prmnk->Qy[2]*prmnk->Qy[2] ) * prmnk->KFilter;
*/
// prmnk->x_base_sin = - prmnk->x_base_sin;
// prmnk->x_base_cos = - prmnk->x_base_cos;
}
