void pll_1p_compute(PLL_1P *v)
{
_iq OMEGA_0 = _IQmpy(6.2831852, v->setFreq);
//------------------------------------//
// OPT Input Notch Filter (2P2Z) //
//------------------------------------//
if(v->Notch_EN == true) {
v->Notch1.In = _IQmpy(v->In, v->out_cosine);
v->Notch1.compute(&v->Notch1);
v->pi1.Ref = v->Notch1.Out;
}
else {
v->pi1.Ref = _IQmpy(v->In, v->out_cosine);
}
//------------------------------------//
// PLL Loop Filter (PI) //
//------------------------------------//
v->pi1.Fdb = 0;
v->pi1.Kp = v->Kp;
v->pi1.Ki_ts = v->Ki_ts;
v->pi1.OutMax = v->OutMax;
v->pi1.OutMin = v->OutMin;
v->pi1.compute(&v->pi1);
OMEGA_0 = OMEGA_0 + v->pi1.Out;
//------------------------------------//
// Integration (1/s) //
//------------------------------------//
v->intg1.In = OMEGA_0;
v->intg1.ClampMax = 6.2831852;
v->intg1.ClampMin = 0;
v->intg1.compute(&v->intg1);
// PLL Outputs
//--------------------------------------
v->out_theta = v->intg1.Out;
v->out_sine = _IQsin(v->out_theta);
v->out_cosine = _IQcos(v->out_theta);
}
void RFFT_f32_sincostable(RFFT_F32_STRUCT *fft)
{
_iq N, tempPI, temp;
Uint16 i, j, k, l;
tempPI = _IQ(0.7853981633975L); // pi/4
k = 1;
l = 0;
for(i = 3; i <= fft->FFTStages; i++)
{
N = _IQ(1.0);
for(j=1; j <= k; j ++)
{
temp = _IQmpy(N, tempPI);
fft->CosSinBuf[l++] = _IQcos(temp);
fft->CosSinBuf[l++] = _IQsin(temp);
N = N + _IQ(1.0);
}
fft->CosSinBuf[l++] = _IQ(0.0);
fft->CosSinBuf[l++] = _IQ(1.0);
k = (k * 2) + 1;
tempPI = _IQmpy(tempPI, _IQ(0.5));
}
}
//----------------------------------------------------------------------------
// Main code:
//----------------------------------------------------------------------------
int main(void)
{
unsigned int i;
_iq tempX, tempY, tempP, tempM, tempMmax;
// char buffer[20];
int *WatchdogWDCR = (void *) 0x7029;
// Disable the watchdog:
asm(" EALLOW ");
*WatchdogWDCR = 0x0068;
asm(" EDIS ");
Step.Xsize = _IQ(STEP_X_SIZE);
Step.Ysize = _IQ(STEP_Y_SIZE);
Step.Yoffset = 0;
Step.X = 0;
Step.Y = Step.Yoffset;
// Fill the buffers with some initial value
for(i=0; i < DATA_LOG_SIZE; i++)
{
Dlog.Xwaveform[i] = _IQ(0.0);
Dlog.Ywaveform[i] = _IQ(0.0);
Dlog.Mag[i] = _IQ(0.0);
Dlog.Phase[i] = _IQ(0.0);
Dlog.Exp[i] = _IQ(0.0);
}
Step.GainX = _IQ(X_GAIN);
Step.FreqX = _IQ(X_FREQ);
Step.GainY = _IQ(Y_GAIN);
Step.FreqY = _IQ(Y_FREQ);
// Calculate maximum magnitude value:
tempMmax = _IQmag(Step.GainX, Step.GainY);
for(i=0; i < DATA_LOG_SIZE; i++)
{
// Calculate waveforms:
Step.X = Step.X + _IQmpy(Step.Xsize, Step.FreqX);
if( Step.X > _IQ(2*PI) )
Step.X -= _IQ(2*PI);
Step.Y = Step.Y + _IQmpy(Step.Ysize, Step.FreqY);
if( Step.Y > _IQ(2*PI) )
Step.Y -= _IQ(2*PI);
Dlog.Xwaveform[i] = tempX = _IQmpy(_IQsin(Step.X), Step.GainX);
Dlog.Ywaveform[i] = tempY = _IQmpy(_IQabs(_IQsin(Step.Y)), Step.GainY);
// Calculate normalized magnitude:
//
// Mag = sqrt(X^2 + Y^2)/sqrt(GainX^2 + GainY^2);
tempM = _IQmag(tempX, tempY);
Dlog.Mag[i] = _IQdiv(tempM, tempMmax);
// Calculate normalized phase:
//
// Phase = (long) (atan2PU(X,Y) * 360);
tempP = _IQatan2PU(tempY,tempX);
Dlog.Phase[i] = _IQmpyI32int(tempP, 360);
// Use the exp function
Dlog.Exp[i] = _IQexp(_IQmpy(_IQ(.075L),_IQ(i)));
// Use the asin function
Dlog.Asin[i] = _IQasin(Dlog.Xwaveform[i]);
}
}