tfr16.c

基于56F8346的异步电机VVVF控制程序。

void tfr16SineWaveGenRDITLQC(tfr16_tSineWaveGenRDITLQ * pSWG, Frac16 * pValues, UInt16 Nsamples)
{
 sSineGenRDITLQ * pState = (sSineGenRDITLQ *) pSWG;
 Frac16       SineAngle;
 Frac16       SineValue1;
 Frac16       SineValue2;
 Frac16       SineDelta;
 Frac16       Sign;
 UInt16       I;
 UInt16       Samples = Nsamples;

 for(I = 0; I < Samples; I++)
 {
  if((pState -> Phase) >= 0)
  {
   if((pState -> Phase) < PI_HALF_PLUS)  /* 0 <= Angle < PI/2 */
   {
    SineAngle = (Frac16) shr((Word16) (pState -> Phase),(Word16) pState -> Shift);
    SineDelta = ((pState -> Phase) & 0x003F);
    Sign      = SWG_SIGN_POSITIVE;
   }

   else  /* PI/2 <= Angle < PI */
   {
    SineAngle = (Frac16) shr((Word16) (PI_PLUS - (pState -> Phase)),(Word16) pState -> Shift);
    SineDelta = (Frac16) ((PI_PLUS - (pState -> Phase)) & 0x003F);
    Sign      = SWG_SIGN_POSITIVE;
   }
  }

  else  /* (*Angle) < 0 */
  {
   if((pState -> Phase) < PI_HALF_MINUS)  /* -PI <= Angle < -PI/2 */
   {
    SineAngle = (Frac16) shr((Word16) (PI_PLUS + (pState -> Phase)),(Word16) pState -> Shift);
    SineDelta = (Frac16) ((PI_PLUS + (pState -> Phase)) & 0x003F);
    Sign      = SWG_SIGN_NEGATIVE;
   }
   else  /* -PI/2 <= Angle < 0 */
   {
    SineAngle = (Frac16) (abs_s(shr((Word16) pState -> Phase,(Word16) pState -> Shift)));
    SineDelta = (Frac16) (abs_s(pState -> Phase) & 0x003F);
    Sign      = SWG_SIGN_NEGATIVE;
   }
  }

  SineValue1 = pState -> pSineTable[SineAngle];
  SineValue2 = pState -> pSineTable[SineAngle + 1];

   * pValues = Sign * (shr((Word16)((SineValue2 - SineValue1) * SineDelta),(Word16) pState -> Shift) + SineValue1);
  pValues += 1;

  if((pState -> Phase + pState -> Delta) >= MAX_16)
  {
   pState -> Phase = sub(MAX_16, pState -> Phase);
   pState -> Phase = sub(pState -> Delta, pState -> Phase);
   pState -> Phase = add(pState -> Phase, SWG_NEG_MAX);
  }

  else
  {
   pState -> Phase = (pState -> Phase + pState -> Delta);
  }
 }
}

typedef struct{
 Frac16 FilterState1;
 Frac16 FilterState2;
 Frac16 FilterCoefs;
}sSineGenDOM;

/*******************************************************************************/
tfr16_tSineWaveGenDOM * tfr16SineWaveGenDOMCreate(Int16   SineFreq,
                 Int16   SampleFreq,
              Frac16  InitialPhasePIx,
              Frac16  Amplitude)
{
 tfr16_tSineWaveGenDOM * pPrivateData = memMallocEM(sizeof(tfr16_tSineWaveGenDOM));

 tfr16SineWaveGenDOMInit(pPrivateData, SineFreq, SampleFreq, InitialPhasePIx, Amplitude);

 return(pPrivateData);
}

/*******************************************************************************/
void tfr16SineWaveGenDOMDestroy(tfr16_tSineWaveGenDOM * pSWG)
{
 if (pSWG != NULL)
 {
  memFreeEM(pSWG);
 }
}

/*******************************************************************************/
void tfr16SineWaveGenDOMInit(tfr16_tSineWaveGenDOM * pSWG,
           Int16                  SineFreq,
           Int16                  SampleFreq,
           Frac16                 InitialPhasePIx,
           Frac16                 Amplitude)
{
 sSineGenDOM * pState = (sSineGenDOM *) pSWG;
 Frac16        Phi;
 Frac16        InitialPhase1;
 Frac16        InitialPhase2;

  Phi = div_s((Frac16)(2 * SineFreq),(Frac16)SampleFreq);

 InitialPhase1 = sub(InitialPhasePIx, Phi);

 if(InitialPhase1 <= MIN_16)
 {
  InitialPhase1 = sub(MIN_16, InitialPhasePIx);
  InitialPhase1 = add(Phi, InitialPhase1);
  Phi = -Phi;
 }

 InitialPhase2 = sub(InitialPhase1, Phi);

 if(InitialPhase2 <= MIN_16)
 {
  InitialPhase2 = sub(MIN_16, InitialPhase1);
  InitialPhase2 = add(Phi, InitialPhase2);
 }

 pState -> FilterState1 = mult(Amplitude, tfr16SinPIx(InitialPhase1));
 pState -> FilterState2 = mult(Amplitude, tfr16SinPIx(InitialPhase2));
 pState -> FilterCoefs  = tfr16CosPIx(Phi);
}

/*******************************************************************************/
void tfr16SineWaveGenDOMC(tfr16_tSineWaveGenDOM * pSWG, Frac16 * pValues, UInt16 Nsamples)
{
 sSineGenDOM * pState = (sSineGenDOM *) pSWG;
 UInt16 I;

 for(I = 0; I < Nsamples; I++)
 {
  * pValues = mult(pState->FilterCoefs, pState -> FilterState1);

  pState -> FilterState2 = mult(SWG_ONEHALF, pState -> FilterState2);

  * pValues = sub(* pValues, pState -> FilterState2);
  * pValues = add(* pValues, * pValues);

  pState -> FilterState2 = pState -> FilterState1;
  pState -> FilterState1 = * pValues;

  pValues += 1;
 }
}

typedef struct{
 Frac16 * pSineTable;
 Frac16   Phase;
 UInt16   SineTableLength;
 UInt16   Shift;
}sWaveGenRDITLQ;

/*******************************************************************************/
tfr16_tWaveGenRDITLQ * tfr16WaveGenRDITLQCreate(Frac16 * pSineTable,
                UInt16   SineTableLength,
                Frac16   InitialPhasePIx)
{
 tfr16_tWaveGenRDITLQ * pPrivateData = memMallocEM(sizeof(tfr16_tWaveGenRDITLQ));

 tfr16WaveGenRDITLQInit(pPrivateData, pSineTable, SineTableLength, InitialPhasePIx);

 return(pPrivateData);
}

/*******************************************************************************/
void tfr16WaveGenRDITLQDestroy(tfr16_tWaveGenRDITLQ * pSWG)
{
 if (pSWG != NULL)
 {
  memFreeEM(pSWG);
 }
}

/*******************************************************************************/
void tfr16WaveGenRDITLQInit(tfr16_tWaveGenRDITLQ * pSWG,
          Frac16             * pSineTable,
          UInt16               SineTableLength,
          Frac16               InitialPhasePIx)
{
 sWaveGenRDITLQ * pState = (sWaveGenRDITLQ *) pSWG;

 pState -> pSineTable      = pSineTable;
 pState -> SineTableLength = SineTableLength;
 pState -> Phase           = InitialPhasePIx;

 pState -> Shift = (UInt16) div_s((Frac16)SineTableLength,(Frac16)SWG_180_DEGREES);
/* pState -> Shift = (norm_s(pState -> Shift)) + 1; */
 pState -> Shift = (UInt16) norm_s((Word16) pState -> Shift);
 pState -> Shift = (pState -> Shift) + 1;
}

/*******************************************************************************/
Frac16 tfr16WaveGenRDITLQC(tfr16_tWaveGenRDITLQ * pSWG, Frac16 PhaseIncrement)
{
 sWaveGenRDITLQ * pState = (sWaveGenRDITLQ *) pSWG;
 Frac16       SineAngle;
 Frac16       SineValue1;
 Frac16       SineValue2;
 Frac16       SineDelta;
 Frac16       LocalPhaseIncrement = PhaseIncrement;
 Frac16       Sign;

 if((pState -> Phase) >= 0)
 {
  if((pState -> Phase) < PI_HALF_PLUS)  /* 0 <= Angle < PI/2 */
  {
   SineAngle = (Frac16) shr((Word16) pState -> Phase,(Word16) pState -> Shift);
   SineDelta = ((pState -> Phase) & 0x003F);
   Sign      = SWG_SIGN_POSITIVE;
  }

  else  /* PI/2 <= Angle < PI */
  {
   SineAngle = (Frac16) shr((Word16) (PI_PLUS - (pState -> Phase)),(Word16) pState -> Shift);
   SineDelta = (Frac16) ((PI_PLUS - (pState -> Phase)) & 0x003F);
   Sign      = SWG_SIGN_POSITIVE;
  }
 }

 else  /* (*Angle) < 0 */
 {
  if((pState -> Phase) < PI_HALF_MINUS)  /* -PI <= Angle < -PI/2 */
  {
   SineAngle = (Frac16) shr((Word16) (PI_PLUS + (pState -> Phase)),(Word16) pState -> Shift);
   SineDelta = (Frac16) ((PI_PLUS + (pState -> Phase)) & 0x003F);
   Sign      = SWG_SIGN_NEGATIVE;
  }
  else  /* -PI/2 <= Angle < 0 */
  {
   SineAngle = (abs_s(shr((Word16) pState -> Phase,(Word16) pState -> Shift)));
   SineDelta = (abs_s((pState -> Phase)) & 0x003F);
   Sign      = SWG_SIGN_NEGATIVE;
  }
 }

 SineValue1 = pState -> pSineTable[SineAngle];
 SineValue2 = pState -> pSineTable[SineAngle + 1];

  SineAngle = Sign * (shr((Word16) ((SineValue2 - SineValue1) * SineDelta), (Word16) pState -> Shift) + SineValue1);

 if((pState -> Phase + LocalPhaseIncrement) >= MAX_16)
 {
  pState -> Phase = sub(MAX_16, pState -> Phase);
  pState -> Phase = sub(LocalPhaseIncrement, pState -> Phase);
  pState -> Phase = add(pState -> Phase, SWG_NEG_MAX);
 }

 else
 {
  pState -> Phase = (pState -> Phase + LocalPhaseIncrement);
 }

 return(SineAngle);
}

typedef struct{
 Frac16 * pSineTable;
 UInt16   SineTableLength;
 UInt16   Shift;
}sSinPIxLUT;
/*******************************************************************************/
tfr16_tSinPIxLUT * tfr16SinPIxLUTCreate(Frac16 * pSineTable,
              UInt16   SineTableLength)
{
 tfr16_tSinPIxLUT * pPrivateData = memMallocEM(sizeof(tfr16_tSinPIxLUT));

 tfr16SinPIxLUTInit(pPrivateData, pSineTable, SineTableLength);

 return(pPrivateData);
}

/*******************************************************************************/
void tfr16SinPIxLUTDestroy(tfr16_tSinPIxLUT * pSWG)
{
 if (pSWG != NULL)
 {
  memFreeEM(pSWG);
 }
}

/*******************************************************************************/
void tfr16SinPIxLUTInit(tfr16_tSinPIxLUT * pSWG,
          Frac16     * pSineTable,
          UInt16       SineTableLength)
{
 sSinPIxLUT * pState = (sSinPIxLUT *) pSWG;

 pState -> pSineTable      = pSineTable;
 pState -> SineTableLength = SineTableLength;

 pState -> Shift = (UInt16) div_s((Frac16)SineTableLength,(Frac16)SWG_180_DEGREES);
/* pState -> Shift = (norm_s(pState -> Shift)) + 1; */
 pState -> Shift = (UInt16) norm_s((Word16) pState -> Shift);
 pState -> Shift = (pState -> Shift) + 1;
}

/*******************************************************************************/
Frac16 tfr16SinPIxLUTC(tfr16_tSinPIxLUT * pSWG, Frac16 PhasePIx)
{
  sSinPIxLUT * pState = (sSinPIxLUT *) pSWG;
 Frac16       SineAngle;
 Frac16       SineValue1;
 Frac16       SineValue2;
 Frac16       SineDelta;
 Frac16       Sign;

 if(PhasePIx >= 0)
 {
  if(PhasePIx < PI_HALF_PLUS)  /* 0 <= Angle < PI/2 */
  {
   SineAngle = (Frac16) shr((Word16) PhasePIx,(Word16) pState -> Shift);
   SineDelta = (PhasePIx & 0x003F);
   Sign      = SWG_SIGN_POSITIVE;
  }

  else  /* PI/2 <= Angle < PI */
  {
   SineAngle = (Frac16) shr((Word16) (PI_PLUS - PhasePIx),(Word16) pState -> Shift);
   SineDelta = (Frac16) ((PI_PLUS - PhasePIx) & 0x003F);
   Sign      = SWG_SIGN_POSITIVE;
  }
 }

 else  /* (*Angle) < 0 */
 {
  if(PhasePIx < PI_HALF_MINUS)  /* -PI <= Angle < -PI/2 */
  {
   SineAngle = (Frac16) shr((Word16) (PI_PLUS + PhasePIx),(Word16) pState -> Shift);
   SineDelta = (Frac16) ((PI_PLUS + PhasePIx) & 0x003F);
   Sign      = SWG_SIGN_NEGATIVE;
  }
  else  /* -PI/2 <= Angle < 0 */
  {
   SineAngle = (abs_s(shr((Word16) PhasePIx,(Word16) pState -> Shift)));
   SineDelta = (abs_s(PhasePIx) & 0x003F);
   Sign      = SWG_SIGN_NEGATIVE;
  }
 }

 SineValue1 = pState -> pSineTable[SineAngle];
 SineValue2 = pState -> pSineTable[SineAngle + 1];

  SineAngle = Sign * (shr((Word16) ((SineValue2 - SineValue1) * SineDelta),(Word16) pState -> Shift) + SineValue1);

 return(SineAngle);
}

typedef struct{
 Frac16 * pSineTable;
 UInt16   SineTableLength;
 UInt16   Shift;
}sCosPIxLUT;
/*******************************************************************************/
tfr16_tCosPIxLUT * tfr16CosPIxLUTCreate(Frac16 * pSineTable,
              UInt16   SineTableLength)
{
 tfr16_tCosPIxLUT * pPrivateData = memMallocEM(sizeof(tfr16_tCosPIxLUT));

 tfr16CosPIxLUTInit(pPrivateData, pSineTable, SineTableLength);

 return(pPrivateData);
}

/*******************************************************************************/
void tfr16CosPIxLUTDestroy(tfr16_tCosPIxLUT * pSWG)
{
 if (pSWG != NULL)
 {
  memFreeEM(pSWG);
 }
}

/*******************************************************************************/
void tfr16CosPIxLUTInitC(tfr16_tCosPIxLUT * pSWG,
          Frac16     * pSineTable,
          UInt16       SineTableLength)
{
 sCosPIxLUT * pState = (sCosPIxLUT *) pSWG;

 pState -> pSineTable      = pSineTable;
 pState -> SineTableLength = SineTableLength;

 pState -> Shift = (UInt16) div_s((Frac16)SineTableLength,(Frac16)SWG_180_DEGREES);
/* pState -> Shift = (norm_s(pState -> Shift)) + 1; */
 pState -> Shift = (UInt16) norm_s((Word16) pState -> Shift);
 pState -> Shift = (pState -> Shift) + 1;
}

/*******************************************************************************/
Frac16 tfr16CosPIxLUTC(tfr16_tCosPIxLUT * pSWG, Frac16 PhasePIx)
{
  sCosPIxLUT * pState = (sCosPIxLUT *) pSWG;
 Frac16       SineAngle;
 Frac16       SineValue1;
 Frac16       SineValue2;
 Frac16       SineDelta;
 Frac16       Sign;

 if(PhasePIx < 0)
 {
  PhasePIx = -PhasePIx;
 }

 PhasePIx -= PI_HALF_PLUS;

 if(PhasePIx >= 0)
 {
  SineAngle = (Frac16) shr((Word16) PhasePIx,(Word16) pState -> Shift);
  SineDelta = (PhasePIx & 0x003F);
  Sign      = SWG_SIGN_NEGATIVE;
 }

 else  /* (*Angle) < 0 */
 {
  SineAngle = (abs_s(shr((Word16) PhasePIx,(Word16) pState -> Shift)));
  SineDelta = (abs_s(PhasePIx) & 0x003F);
  Sign      = SWG_SIGN_POSITIVE;
 }

 SineValue1 = pState -> pSineTable[SineAngle];
 SineValue2 = pState -> pSineTable[SineAngle + 1];

  SineAngle = Sign * (shr((Word16) ((SineValue2 - SineValue1) * SineDelta),(Word16) pState -> Shift) + SineValue1);

 return(SineAngle);
}