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