/** ###################################################################
**     THIS BEAN MODULE IS GENERATED BY THE TOOL. DO NOT MODIFY IT.
**     Filename  : MC1.C
**     Project   : bldc_hall_demo
**     Processor : MC56F8006_48_LQFP
**     Beantype  : MC_PIController
**     Version   : Bean 01.016, Driver 01.03, CPU db: 3.00.177
**     Compiler  : Metrowerks DSP C Compiler
**     Date/Time : 28.11.2008, 16:30
**     Abstract  :
**          This bean encapsulate the API for standard controllers (e.g., PI and PID)
**          for use in motor control applications in general.
**     Settings  :
**          This bean has no settings.
**     Contents  :
**         controllerPItype1_asmSc - asm Frac16 MC1_controllerPItype1_asmSc(Frac16 DesiredValue, Frac16...
**
**     (c) Freescale Semiconductor
**     2004 All Rights Reserved
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/

/* MODULE MC1. */

#include "MC1.h"
#include "prototype.h"

/*
** ===================================================================
**     Method      :  MC1_controllerPItype1_asmSc (bean MC_PIController)
**
**     Description :
**         This function calculates PI controller algorithm with
**         fractional data format..
**     Parameters  :
**         NAME            - DESCRIPTION
**         DesiredValue    - Desired value for
**                           controller.
**         MeasuredValue   - Actual value, input
**                           error = DesiredValue - MeasuredValue.
**       * pParams         - Pointer to the structure of the
**                           PI controller parameters.
**     Returns     :
**         ---             - This function returns controller output
**                           in fractional data format (16 bit).
** ===================================================================
*/
asm Frac16 MC1_controllerPItype1_asmSc(Frac16 DesiredValue,Frac16 MeasuredValue,mc_sPIparams1_limitSc *pParams)
{
  /*
  SP     ProportionalPortion MSP
  SP-1   ProportionalPortion LSP
  SP-2   OMR register
  Y0     DesiredValue
  Y1     MeasuredValue
  R2     base address of the mc_sPIparams1_limitSc structure
  */
  adda #4,SP                           /* create stack */
  move.w OMR,X:(SP-0x0002)             /* save OMR reg */
  bfset #0x10,OMR                      /* set saturation mode */
  nop                                  /* nop inserted due to pipeline conflict avoidance */
  nop                                  /* nop inserted due to pipeline conflict avoidance */

  /****** proportional portion calculation - begin ******/
  sub Y1,Y0                            /* Y0 <= Y0 - Y1 */
                                       /* Y0 <= DesiredValue - MeasuredValue */
  move.w X:(R2),A                      /* A <= ProportionalGain */
  mpy Y0,A1,A                          /* A <= Y0*A1 */
                                       /* A <= ProportionalGain * (DesiredValue - MeasuredValue) */

  /* arithmetic left or right shift according to ProportionalGainScale
     value, member of the mc_sPIparams1_limitSc structure */
  move.w X:(R2+0x0006),B               /* B <= ProportionalGainScale */
  asrr.l B,A                           /* A <= A >> B (arithmetically) */

  move.w A,X:(SP)                      /* the MSP of the ProportionalPortion to SP  */
  move.w A0,X:(SP-0x0001)              /* the LSP of the ProportionalPortion to SP-1 */
  /****** proportional portion calculation - end ******/

  /****** integral portion calculation - begin ******/
  move.w X:(R2+1),A                    /* A <= IntegralGain */
  mpy Y0,A1,A                          /* A <= Y0*A1 */
                                       /* A <= IntegralGain * (DesiredValue - MeasuredValue) */
  /* arithmetic left or right shift according to IntegralGainScale
     value, member of the mc_sPIparams1_limitSc structure */
  move.w X:(R2+0x0007),B               /* B < IntegralGainScale */
  asrr.l B,A                           /* A <= A >> B (arithmetically) */
  move.w X:(R2+0x0003),B               /* MSP of the IntegralPortionK_1 to B  */
  move.w X:(R2+0x0002),B0              /* LSP of the IntegralPortionK_1 to B0  */
  add A,B                              /* B <= A + B */
                                       /* B <= IntegralGain * (DesiredValue - MeasuredValue)
`` ```````````````````````````                                              + IntegralPortionK_1, result in B  */
  /****** integral portion calculation - end ******/

  /****** integral portion limitation - begin ******/
  move.w X:(R2+4),X0                   /* X0 <= PositivePILimit */
  move.w X:(R2+5),Y1                   /* Y1 <= NegativePILimit */
  cmp X0,B                             /* CCR <= cond(Src - RegHi) */
  tge X0,B                             /* if Src - RegHi >= 0 then Src <= RegHi */
  cmp Y1,B                             /* CCR <= cond(Src - RegLo) */
  tlt Y1,B                             /* if Src - RegLo < 0 then Src <= RegLo */
  move.w B,X:(R2+0x0003)               /* MSP of the IntegralPortionK from B
``````                                    to IntegralPortionK_1  */
  move.w B0,X:(R2+0x0002)              /* LSP of the IntegralPortionK from B0
      ```````                             to IntegralPortionK_1  */

  IntegralPortionNegLimit:
  /****** integral portion limitation - end ******/

  /****** controller output calculation - begin ******/
  move.w X:(R2+0x0003),B               /* MSP of the IntegralPortionK_1 to B */
  move.w X:(R2+0x0002),B0              /* LSP of the IntegralPortionK_1 to B0 */
  move.w X:(SP),A                      /* MSP of the ProportionalPortion to A  */
  move.w X:(SP-0x0001),A0              /* LSP of the ProportionalPortion to A0  */
  add A,B                              /* B <= A + B */
                                       /* B <= IntegralPortionK + ProportionalPortion */
  cmp X0,B                             /* CCR <= cond(Src - RegHi) */
  tge X0,B                             /* if Src - RegHi >= 0 then Src <= RegHi */
  cmp Y1,B                             /* CCR <= cond(Src - RegLo) */
  tlt Y1,B                             /* if Src - RegLo < 0 then Src <= RegLo */
  move.w B,Y0                          /* Y0 <= B1 */
                                       /* Y0 is returned by the function */
  /****** controller output calculation - end ******/
  moveu.w     X:(SP-0x0002),OMR        /* restore OMR reg */
  nop                                  /* nop inserted due to pipeline conflict avoidance */
  adda #-4,SP                          /* release stack */
  rts
}

/* END MC1. */

/*
** ###################################################################
**
**     This file was created by UNIS Processor Expert 2.99 [04.17]
**     for the Freescale 56800 series of microcontrollers.
**
** ###################################################################
*/
