/*
** #################################################################################################################
**     THIS MODULE IS A PART OF PROCESSOR EXPERT SYSTEM LIBRARY.
**
**     Filename  : Decoder.H
**
**     Family    : Freescale DSP
**
**     Compiler  : Freescale DSP C Compiler
**
**     Abstract  :
**
**         This PESL module implements a Decoder.
**
**         Implemented devices:
**
**           ENC - Quadrature Encoder/Decoder
**
**         Supported CPUs:
**           MC56F84441VLF,MC56F84442VLH,MC56F84451VLF,MC56F84452VLH,MC56F84462VLH,MC56F84565VLK,MC56F84567VLL,MC56F84585VLK,MC56F84587VLL,MC56F84766VLK,MC56F84769VLL,MC56F84786VLK,MC56F84789VLL
**
**     (c) Freescale Semiconductor
**     2012 All Rights Reserved
**
**     http      : www.freescale.com
**
** #################################################################################################################
*/

#include "PE_Types.h"
#include "DECODER.h"

#if (CPUtype == _MC56F84441VLF) || (CPUtype == _MC56F84442VLH) || (CPUtype == _MC56F84451VLF) || (CPUtype == _MC56F84452VLH) || (CPUtype == _MC56F84462VLH) || (CPUtype == _MC56F84565VLK) || (CPUtype == _MC56F84567VLL) || (CPUtype == _MC56F84585VLK) || (CPUtype == _MC56F84587VLL) || (CPUtype == _MC56F84766VLK) || (CPUtype == _MC56F84769VLL) || (CPUtype == _MC56F84786VLK) || (CPUtype == _MC56F84789VLL)

#include "PESLlib.h"
#include "periph.h"
#include "math.h"
#include "prototype.h"
#include "dspfunc.h"

#ifdef __cplusplus
extern "C" {
#endif


/*******************************************************

   Predefined basic intrinsics.

   Builtin support for these functions will be implemented
   in the CodeWarrior C compiler code generator in Release 3.0.

   The intrinsic functions are defined in the compiler
   defined functions name space. They are redefined here
   according to the ETSI naming convention.

 ******************************************************/


/********************************************************************************
   Functions are taken from Freescale DSP56800 QuickStart 1.0
   and modified for using in PESL macros.
********************************************************************************/


/********************************************************************************
 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( (Word16)pEncScale->EncPulses, 0x0001 );
  }
  else
  {
      ltmp = L_mult( (Word16)pEncScale->EncPulses, (Word16)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( (Word16)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( (Word16)pEncScale->EncPulses, 0x0002 );
  pEncScale->normPosCoef = sub( 30, norm_l( ltmp ) );
  ultmp = (UInt32)L_shl( 0x00000001, (Word16)pEncScale->normPosCoef);
  ustmp = (UInt16)extract_l( (Word32)ultmp );

  /* not working yet */
  ultmp = impyuu( 0xffff, ustmp );

  ltmp = ((long)(ultmp)) / ltmp;
  pEncScale->scalePosCoef = (UInt16)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 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 = (Word16)periphMemRead(&pDecBase->rev);  //RevolutionCounterReg

    /* read the current position */
    positionReg.RegParts.LSBpart = periphMemRead(&pDecBase->lposh);  //LowerPositionHoldReg
    positionReg.RegParts.MSBpart = (Word16)periphMemRead(&pDecBase->uposh);  //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 = (UInt16)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(pDECposd);  //PositionDifferenceReg
   stmp = (Word16) (periphMemRead(&pDecBase->posdh)); //to overcome some issues on certain device rev.


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

   return( stmp );
}

#ifdef __cplusplus
}
#endif

#endif /* CPUtype */
