C++ (Cpp) _IQatan2PU Examples

Example #1

File: smopos.c Project: victor-zheng/Four_in_One

void smopos_calc(SMOPOS *v)
{	
    _iq E0;
    
    E0 = _IQ(0.5);
    
// Sliding mode current observer
    v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha-v->Ealpha-v->Zalpha));
    v->EstIbeta = _IQmpy(v->Fsmopos,v->EstIbeta) + _IQmpy(v->Gsmopos,(v->Vbeta-v->Ebeta-v->Zbeta));

// Current errors
    v->IalphaError = v->EstIalpha - v->Ialpha;
    v->IbetaError = v->EstIbeta - v->Ibeta;

// Sliding control calculator
    if (_IQabs(v->IalphaError) < E0)
       v->Zalpha = _IQmpy(v->Kslide,_IQdiv(v->IalphaError,E0));  
    else if (v->IalphaError >= E0) 
       v->Zalpha = v->Kslide;
    else if (v->IalphaError <= -E0) 
       v->Zalpha = -v->Kslide;

    if (_IQabs(v->IbetaError) < E0)
       v->Zbeta = _IQmpy(v->Kslide,_IQdiv(v->IbetaError,E0));  
    else if (v->IbetaError >= E0) 
       v->Zbeta = v->Kslide;
    else if (v->IbetaError <= -E0) 
       v->Zbeta = -v->Kslide;

// Sliding control filter -> back EMF calculator
    v->Ealpha = v->Ealpha + _IQmpy(v->Kslf,(v->Zalpha-v->Ealpha));
    v->Ebeta = v->Ebeta + _IQmpy(v->Kslf,(v->Zbeta-v->Ebeta));

// Rotor angle calculator -> Theta = atan(-Ealpha,Ebeta)
    v->Theta = _IQatan2PU(-v->Ealpha,v->Ebeta); 
}

Example #2

File: IQsampleSmall.c Project: microdu/Energy_Pack_CODE

//----------------------------------------------------------------------------
// 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]);
    }
}