decoder.c

MC56F802BLDC 可以使用的算法 就是电机启动有点慢

/******************************************************************************
*
* Motorola Inc.
* (c) Copyright 2001 Motorola, Inc.
* ALL RIGHTS RESERVED.
*
*******************************************************************************
*
* FILE NAME: decoder.c
*
* DESCRIPTION: Sorce file for Quadrature Decoder driver
*
* MODULES INCLUDED: types.h
*                   periph.h
*                   arch.h
*                   appconfig.h
*                   decoder.h
*                   
* NOTES:
*
*******************************************************************************/

#include "types.h"
#include "periph.h"
#include "arch.h"

#include "appconfig.h"
#include "decoder.h"


/********************************************************************************
 decoderInit() function performs the Quadrature Decoder module static 
 configuration based on the configurable items from appconfig.h 
********************************************************************************/
void decoderInit(arch_sDecoder *pDecBase)
{
           if(pDecBase == DEC_0)
           {  
               #ifdef DEC_0_WATCHDOG_TIMEOUT_REG
                  	periphMemWrite(DEC_0_WATCHDOG_TIMEOUT_REG, &ArchIO.Decoder0.WatchdogTimeoutReg);
               #endif
               #ifdef DEC_0_FILTER_INTERVAL_REG
                  	periphMemWrite(DEC_0_FILTER_INTERVAL_REG, &ArchIO.Decoder0.FilterIntervalReg);
               #endif
               #ifdef DEC_0_UPPER_INIT_REG
                  	periphMemWrite(DEC_0_UPPER_INIT_REG, &ArchIO.Decoder0.UpperInitializationReg);
               #endif
               #ifdef DEC_0_LOWER_INIT_REG
                  	periphMemWrite(DEC_0_LOWER_INIT_REG, &ArchIO.Decoder0.LowerInitializationReg);
               #endif
               #ifdef DEC_0_CONTROL_REG
                  	periphMemWrite(DEC_0_CONTROL_REG, &ArchIO.Decoder0.ControlReg);
               #endif
            }
            #if( defined(DSP56F805) || defined(DSP56F807) )
            else if(pDecBase == DEC_1)
            {
               #ifdef DEC_1_WATCHDOG_TIMEOUT_REG
                  	periphMemWrite(DEC_1_WATCHDOG_TIMEOUT_REG, &ArchIO.Decoder1.WatchdogTimeoutReg);
               #endif
               #ifdef DEC_1_FILTER_INTERVAL_REG
                  	periphMemWrite(DEC_1_FILTER_INTERVAL_REG, &ArchIO.Decoder1.FilterIntervalReg);
               #endif
               #ifdef DEC_1_UPPER_INIT_REG
                  	periphMemWrite(DEC_1_UPPER_INIT_REG, &ArchIO.Decoder1.UpperInitializationReg);
               #endif
               #ifdef DEC_1_LOWER_INIT_REG
                  	periphMemWrite(DEC_1_LOWER_INIT_REG, &ArchIO.Decoder1.LowerInitializationReg);
               #endif
               #ifdef DEC_1_CONTROL_REG
                  	periphMemWrite(DEC_1_CONTROL_REG, &ArchIO.Decoder1.ControlReg);
               #endif			
            }
            #endif
            
         return;
}

	    

/********************************************************************************
 decoderCoefCalc() function calculates the scaling coeficients needed for the correct
 functionality of decoderGetScaledPosition() and decoderGetScaledPositionDifference()
 functions. Note, this function must be called before the function call of the above
 mentioned functions.
 decoder_sEncScale structure memebers needed to fill prior calling this routine:
     EncPulses ... the number of encoder pulses
     RevolutionScale ... the number of revoulutions to represent the register full range
       ( this variable has only meaning for decoderGetScaledPositionDifference() function ) 
			                0 ->  +/- PI
							1 ->  +/- 2PI
							2 ->  +/- 4PI
********************************************************************************/
void decoderCoefCalc( decoder_sEncScale *pEncScale )
{
  Word16 stmp, stmp1;
  Word32 ltmp;
  UInt16 ustmp;
  UInt32 ultmp;


  /* scaling for the Position Difference Counter Register */
  if( pEncScale->RevolutionScale == 0 )
  {
	  ltmp = L_mult( pEncScale->EncPulses, 0x0001 );
  }
  else
  {
      ltmp = L_mult( pEncScale->EncPulses, pEncScale->RevolutionScale );
      ltmp = L_shl( ltmp, 1 );
  }

  if( ltmp < 0x00007fffL )
  {
     stmp = extract_l( ltmp );
     pEncScale->normDiffPosCoef = sub( 14, norm_s( stmp ) );
     // stmp1 = shl( 0x0001, normBits);
     stmp1 = 0x0001 << (pEncScale->normDiffPosCoef);
     ltmp = L_mult( 0x7fff, stmp1 );
  
     ltmp = ltmp / stmp;
     pEncScale->scaleDiffPosCoef = (Int16)(ltmp / 2 );
  }
  else
  {
     pEncScale->normDiffPosCoef = 0;
     pEncScale->scaleDiffPosCoef = 0;
  }
  
  
   /* scaling for Position Counter Register */
  ltmp = L_mult( pEncScale->EncPulses, 0x0002 );
  pEncScale->normPosCoef = sub( 30, norm_l( ltmp ) );
  ultmp = L_shl( 0x00000001, pEncScale->normPosCoef);
  ustmp = extract_l( ultmp );
  ultmp = impyuu( 0xffff, ustmp );
	
  ltmp = ((long)(ultmp)) / ltmp;
  pEncScale->scalePosCoef = extract_l( ltmp );
  return;
}

/********************************************************************************
 decoderGetScaledPosition() calculates the absolute position. It returns a 32bit
 result where the upper part represesnts the number of revolutions while the lower
 part represents the portion of the current revolution scaled into the 16bit
 unsigned data range.
 Note, the decoderCoefCalc() function must be called prior to the call of this function.
 Attention!  This function forces to hold all data registers when called.
********************************************************************************/
Word32 decoderGetScaledPosition(arch_sDecoder *pDecBase, decoder_sEncScale *pEncScale)
{
    decoder_uReg32bit position, positionReg;
    Int32 iltemp;
	UWord16 revolutionPosition;		/* position inside one revolution */
	Word16 tmpRevolution;
    Word16 sign;            /* direction of rotation */

    
	/* reading of the registers content */
	/* read number of revolutions, create MSB part of the result, i.e. number of completed rotations */
    position.RegParts.MSBpart = periphMemRead( (UWord16 *)(&((arch_sDecoder*)pDecBase)->RevolutionCounterReg ) );
        
    /* read the current position */
    positionReg.RegParts.LSBpart = periphMemRead( (UWord16 *)(&((arch_sDecoder*)pDecBase)->LowerPositionHoldReg));
    positionReg.RegParts.MSBpart = periphMemRead( (UWord16 *)(&((arch_sDecoder*)pDecBase)->UpperPositionHoldReg));
    
    sign = positionReg.RegParts.MSBpart;  /* to distinguish direction of rotation */
    
    /* convert negative values to positive ones for the subsequent calculations */
    if( sign < 0 ) positionReg.Reg32bit = L_abs(positionReg.Reg32bit);
    
    revolutionPosition = (UWord16)positionReg.RegParts.LSBpart;

	/* scale of revolutionPosition */
    iltemp = (Int32)( impyuu(pEncScale->scalePosCoef, revolutionPosition) );
	iltemp = L_shr(iltemp, pEncScale->normPosCoef);
	
	/* convert back to negative values if originally negative direction */
	if( sign < 0 ) iltemp = L_negate( iltemp );

	revolutionPosition = extract_l(iltemp);
	/* create LSB part of the result, i.e. portion of 1 rotation scaled into 16bits */
    position.RegParts.LSBpart = revolutionPosition;
    
	return( position.Reg32bit );	
}	

/********************************************************************************
 decoderGetScaledPositionDifference() returns the scaled relative position - difference.
 The 16bit signed value represents the range determined by the arguments of decoderCoefCalc()
 function.
 Note, the decoderCoefCalc() function must be called prior to the call of this function.
 Attention!  This function forces to hold all data registers when called.
********************************************************************************/
Word16 decoderGetScaledPositionDifference(arch_sDecoder *pDecBase, decoder_sEncScale *pEncScale)
{
   Word16 stmp;
   Word32 ltmp;

   stmp = periphMemRead( (UWord16 *)(&((arch_sDecoder*)pDecBase)->PositionDifferenceReg) );

      
   ltmp = L_mult( stmp, pEncScale->scaleDiffPosCoef );
   stmp = pEncScale->normDiffPosCoef + 1;
   ltmp = L_shr( ltmp, stmp );
   stmp = extract_l( ltmp );
      
   return( stmp );
}