sdkmath.dbg

BLDC_SENSORLESS controller using freeescale mcu aw60

;/*****************************************************************************
;*
;* Motorola Inc.
;* (c) Copyright 2001 Motorola, Inc.
;* ALL RIGHTS RESERVED.
;*
;******************************************************************************
;*
;* File Name: sdkmath.asm
;*
;* Description:
;*   File contains basic mathematical functions
;*
;* Modules Included: 
;*   SWord16 add (SWord16 x, SWord16 y)
;*   SWord16 sub (SWord16 x, SWord16 y)
;*   SWord16 neg (SWord16 x)
;*   unsigned int uinv_16(unsigned int x)
;*
;*****************************************************************************/

;/*****************************************************************************
;*   External Symbol Definition
;*****************************************************************************/
 XDEF add
 XDEF neg
 XDEF sub
 XDEF uinv_16

;/*****************************************************************************
;*
;* Module: SWord16 add(SWord16 x, SWord16 y)
;*
;* Description:
;*   The function performs the addition x+y with overflow control
;*   and saturation. The 16-bit result is set at +32767 when overflow occurs,
;*   or at -32768 when underflow occurs.
;*
;* Returns:   x + y
;*
;* Arguments: x (in) L->4,SP H->3,SP
;*            y (in) L->X;   H->H
;*
;* Range Issues: None
;*
;* Special Issues: The result is saturated
;*
;*****************************************************************************/
;  /* (V - Overflow Flag, N - Negative flag) */
;  /* V=1 & N=1 too high */
;  /* V=1 & N=0 too low  */ 

add:    TXA
        ADD  4,SP       ;          /* x + y */
        PSHA
        PSHH
        PULA
        ADC  4,SP
        PSHA
        PULH
        PULX
        BLT  addtlow    ;V^N:      /* can be too low */
        BPL  addret1    ;V=0 & N=0
        LDHX #7FFFh      ;V=1 & N=1 /* too high */
addret1:RTS
addtlow:BMI  addret2    ;V=0 & N=1
        LDHX #8000h			;V=1 & N=0 /* too low */
addret2:RTS

;/*****************************************************************************
;*
;* Module: SWord16 neg(SWord16 x)
;*
;* Description:
;*   The function performs the negation of x with overflow control
;*   and saturation. The 16-bit result is set at +32767 when overflow occurs.
;*
;* Returns:   -x
;*
;* Arguments: x (in) L->(X)A; H->H(X)
;*
;* Range Issues: None
;*
;* Special Issues: The result is saturated
;*
;*****************************************************************************/
;  /* (V - Overflow Flag, N - Negative flag) */
;  /* V=1 & N=1 too high */
neg:  
        TXA
        PSHH
        PULX
        COMX           ;        /* x = -x */
        NEGA
        BNE  endneg
        INCX
        BPL  endneg    ;N=0     /* x was negative but overflow did not ocure */
        BLT  endneg    ;N=1 V=0 /* x was positive */
        DECA           ;N=1 V=1 /* to high */
        DECX
endneg: PSHX
        PULH
        TAX
        RTS

;/*****************************************************************************
;*
;* Module: SWord16 sub(SWord16 x, SWord16 y)
;*
;* Description:
;*   The function performs the subtraction x-y with overflow control
;*   and saturation. The 16-bit result is set at +32767 when overflow occurs,
;*   or at -32768 when underflow occurs.
;*
;* Returns:   x - y
;*
;* Arguments: x (in) L->4,SP H->3,SP
;*            y (in) L->X(A);   H->H(X)
;*
;* Range Issues: None
;*
;* Special Issues: The result is saturated
;*
;*****************************************************************************/
;  /* (V - Overflow Flag, N - Negative flag) */
;  /* V=1 & N=1 too high */
;  /* V=1 & N=0 too low  */ 
sub:    PSHX
        PSHH
        TSX
        LDA  5,X        ;x - y
        SUB  1,X
        PSHA
        LDA  4,X
        SBC  ,X
        PSHA
        PULH
        PULX
        AIS  #2
        BLT  subtlow    ;V^N:      /* can be too low */
        BPL  subret1    ;V=0 & N=0
        LDHX #7FFFh     ;V=1 & N=1 /* too high */
subret1:RTS
subtlow:BMI  subret2    ;V=0 & N=1
        LDHX #8000h     ;V=1 & N=0 /* too low */        
subret2:RTS

;/*****************************************************************************
;*
;* Module: unsigned int uinv_16(unsigned int x)
;*
;* Description: unsigned int inverse function 1/x
;*   
;*
;* Returns: 16777216*1/x
;*
;* Global Data:None
;*
;* Arguments: x (in) L->X H->H
;*            
;*
;* Range Issues: if x < 256 returns 0xFFFF
;*               
;*
;* Special Issues: None
;*
;*****************************************************************************/
uinv_16: 
        PSHH
        PULA
        TSTA
        BEQ  uinvzr			 ;/* if (x<256 return 0xFFFF)*/
        PSHX
        PSHH
        LDA #0FFh
        PSHA
        PSHA
        LDA #0
        PSHA
        PSHA
        LDA #9
        PSHA
uinvsc: TST 6,SP
        BMI uinvcn
        ASL 7,SP
        ROL 6,SP
        INC 1,SP
        BRA uinvsc
uinvcn: LDHX 4,SP
        CPHX 6,SP
        BLO uinvsm
uinvof: LDA 5,SP
        SUB 7,SP
        STA 5,SP
        LDA 4,SP
        SBC 6,SP
        STA 4,SP
        INC 3,SP
uinvsm: DEC 1,SP
        BEQ uinvrz
        ASL 3,SP
        ROL 2,SP
        ASL 5,SP
        ROL 4,SP
        BLO uinvof
        BRA uinvcn
uinvrz: LDHX 2,SP        
        AIS #7
        BRA invok
uinvzr: LDX  #0FFh
        TXA
        PSHA
        PULH
invok:  RTS