resolver.c

有关TI公司用于交流电机控制程序（包含文档以及源码）

/*=====================================================================================
 File name:        RESOLVER.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:  Speed and position measurement computation based resolver                   

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/

#include "IQmathLib.h"         // Include header for IQmath library 
// Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file 
#include "dmctype.h"
#include "resolver.h"

void resolver_calc(RESOLVER *v)
{

   demodulator_calc(v);
   filter_calc(v);
   position_speed_calc(v);
   angle_comp_calc(v);

}


void demodulator_calc(RESOLVER *v)
{

// Inputs: v->SinIn, v->CosIn
// Outputs: v->DemodSin, v->DemodCos

   int16 i;

   // Pass the previous resolver signals   
   for(i=v->DemodConst-1;i>0;i--)  {
      v->PreviousDemodSin[i] = v->PreviousDemodSin[i-1];
      v->PreviousDemodCos[i] = v->PreviousDemodCos[i-1];
   }
   
   // Index zero in the array keeps the current resolver signals
   v->PreviousDemodSin[0] = v->SinIn;
   v->PreviousDemodCos[0] = v->CosIn;
   
   // Initialize the demodulated signals
   v->DemodSin = 0;
   v->DemodCos = 0;

   // Demodulate the resolver signals
   for(i=0;i<v->DemodConst;i++)  {
     if (_IQabs(v->PreviousDemodSin[i]) > v->DemodSin)
        v->DemodSin = _IQabs(v->PreviousDemodSin[i]);
     if (_IQabs(v->PreviousDemodCos[i]) > v->DemodCos)
        v->DemodCos = _IQabs(v->PreviousDemodCos[i]);
   }
}


void filter_calc(RESOLVER *v)
{

// Inputs: v->DemodSin, v->DemodCos
// Outputs: v->FilteredSin, v->FilteredCos

   int16 i;

   // Initialize the filtered signals
   v->FilteredSin = 0;
   v->FilteredCos = 0;

   for(i=17;i>=0;i--)  {
     
     if (i>0) {
       // Pass the previous demodulated signals
       v->PreviousSin[i] = v->PreviousSin[i-1];
       v->PreviousCos[i] = v->PreviousCos[i-1];
     }  
     else  {
       // Index zero in the array keeps the current demodulated signals
       v->PreviousSin[0] = v->DemodSin;
       v->PreviousCos[0] = v->DemodCos;
     }

     // Filtering the signals 
     v->FilteredSin += _IQmpy(v->FilterGain[i],v->PreviousSin[i]);
     v->FilteredCos += _IQmpy(v->FilterGain[i],v->PreviousCos[i]); 

   }
}


void position_speed_calc(RESOLVER *v)
{

// Inputs: v->FilteredSin, v->FilteredCos
// Outputs: v->ElecTheta, v->MechTheta, v->RefTheta, v->Speed

   _iq Tmp1, Tmp2, FilterGain;

   // Amplify the filtered signals
   v->AmplifiedSin = _IQmpy(v->SignalGain,v->FilteredSin);
   v->AmplifiedCos = _IQmpy(v->SignalGain,v->FilteredCos);

   // Arctan function
   Tmp1 = _IQatan2PU(v->AmplifiedSin,v->AmplifiedCos);
   Tmp2 = _IQatan2PU(v->AmplifiedCos,v->AmplifiedSin);

   // Low-pass filter for resolver angles 
   FilterGain = _IQ(0.95);
   v->FilterTheta1 = _IQmpy(FilterGain,v->FilterTheta1) + _IQmpy((_IQ(1)-FilterGain),Tmp1);
   v->FilterTheta2 = _IQmpy(FilterGain,v->FilterTheta2) + _IQmpy((_IQ(1)-FilterGain),Tmp2);

   // Rotor speed computation
   if ((v->FilterTheta1 < DIFF_MAX_LIMIT)&(v->FilterTheta1 > DIFF_MIN_LIMIT))
		// GLOBAL_Q = Q21*(GLOBAL_Q-GLOBAL_Q)
       Tmp1 = _IQ21mpy(v->K1,_IQabs(v->FilterTheta1 - v->OldTheta));
   else  Tmp1 = v->Speed;

   // Low-pass filter for calculated speed
   FilterGain = _IQ(0.995);
   v->Speed = _IQmpy(FilterGain,v->Speed) + _IQmpy((_IQ(1)-FilterGain),Tmp1);

   // Limit the calculated speed within (-1,1)
   if (v->Speed > _IQ(1))
       v->Speed = _IQ(1);
   else if (v->Speed < _IQ(-1))
      v->Speed = _IQ(-1);

   // Update the mechanical angle
   v->OldTheta = v->FilterTheta1;

   // Produce the bipolar signal of theta     
   v->FilterTheta = v->FilterTheta1 - v->FilterTheta2;

   // Check the sign of filtered theta
   if (v->FilterTheta > _IQ(0))
      v->SignFlag = 0;
   if (v->FilterTheta <= _IQ(0))
      v->SignFlag = 1;

   // Detecting the zero-crossing of theta signal using XOR operation
   Tmp2 = v->SignFlag^v->OldSignFlag;

   // A zero-crossing happens 
   if (Tmp2)   {
      v->ThetaCounter += 1;

      if (v->ThetaCounter == v->ThetaCounterMax)  {
         v->ThetaCounter = 0;
         // Increase ThetaCounterMax to a high number to avoid the repeated sign change during zero crossings
         v->ThetaCounterMax = 1000;  

         v->SignalCounter++;
         if (v->SignalCounter == 4)
           v->SignalCounter = 0;

         if (v->SignalCounter == 0)  {
            v->MechTheta = _IQ(0);
            v->RefTheta = _IQ(0);
         }
         else if (v->SignalCounter == 1)  {
           Tmp1 = _IQ(0.25) + _IQmpy(v->SpeedGain,v->Speed);
           v->MechTheta = _IQmpy((v->MechTheta + _IQ(0.25) + Tmp1),_IQ(0.3333));
         }
         else if (v->SignalCounter == 2)  {
           Tmp1 = _IQ(0.50) + _IQmpy(v->SpeedGain,v->Speed);
           v->MechTheta = _IQmpy((v->MechTheta + _IQ(0.50) + Tmp1),_IQ(0.3333));
         }
         else if (v->SignalCounter == 3)  {
           Tmp1 = _IQ(0.75) + _IQmpy(v->SpeedGain,v->Speed);
            v->MechTheta = _IQmpy((v->MechTheta + _IQ(0.75) + Tmp1),_IQ(0.3333));
         }
       }
   }

   if (_IQabs(v->FilterTheta) > _IQ(0.1))  {
      v->ThetaCounterMax = v->CounterMax;
      v->ThetaCounter = 0;
   }

    v->OldSignFlag = v->SignFlag;

    v->MechTheta += _IQmpy(v->SpeedGain,v->Speed);

    if (v->MechTheta >= _IQ(1))
       v->MechTheta = _IQ(1);
    if (v->MechTheta < _IQ(0))
       v->MechTheta = _IQ(0);

    // Low-pass filter 
    FilterGain = _IQ(0.70);
    v->RefTheta = _IQmpy(FilterGain,v->RefTheta) + _IQmpy((_IQ(1)-FilterGain),v->MechTheta); 

    // Compute electrical angle
    v->ElecTheta = v->MechTheta*v->PolePairs;
    if (v->ElecTheta >= _IQ(1))
       v->ElecTheta -= _IQ(1);

   // Change motor speed from pu value to rpm value (Q15 -> Q0)
   // Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
    v->SpeedRpm = _IQmpy(v->BaseRpm,v->Speed); 
}


void angle_comp_calc(RESOLVER *v)
{

// Inputs: v->RefTheta
// Outputs: v->OutputTheta

    // Convert OutputTheta to from mechanical to electrical angle  
    v->OutputTheta = v->RefTheta*v->PolePairs;

    if (v->OutputTheta >= _IQ(1))
       v->OutputTheta -= _IQ(1);

    // Compensate the mechanical angle between resolver signal and back EMF of winding a
    v->OutputTheta += v->CalibratedAngle;

    if (v->OutputTheta >= _IQ(1))
       v->OutputTheta -= _IQ(1);

    // (0.125 pu)*v->PolePairs pu = (45 deg)*v->PolePairs  electrical degree 
     v->OutputTheta += _IQ1mpy(v->PolePairs,_IQ25(0.125));

    if (v->OutputTheta >= _IQ(1))
       v->OutputTheta -= _IQ(1);

    // _IQ(0.171) = Mechanical phase delay at base speed due to low-pass filter (FIR 17-order)
     v->OutputTheta += _IQmpy(v->Speed,_IQ1mpy(v->PolePairs,_IQ25(0.171)));

    if (v->OutputTheta >= _IQ(1))
     v->OutputTheta -= _IQ(1);

    // _IQ(0.09) = Mechanical phase delay at base speed due to low-pass filter (1-order)
     v->OutputTheta += _IQmpy(v->Speed,_IQ1mpy(v->PolePairs,_IQ25(0.09)));

    if (v->OutputTheta >= _IQ(1))
      v->OutputTheta -= _IQ(1);
}