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