📄 pidmath.asm
字号:
;********************************************************
;This file contains the following math routines:
;24-bit addittion
;24-bit subtraction
;16*16 Unsigned Multiply
;24/16 Unsigned Divide
list p=18F452
#include <p18F452.inc>
#define _Z STATUS,2
#define _C STATUS,0
GLOBAL AARGB0,AARGB1,AARGB2,AARGB3
GLOBAL BARGB0,BARGB1,BARGB2,BARGB3
GLOBAL ZARGB0,ZARGB1,ZARGB2
GLOBAL REMB0,REMB1
GLOBAL TEMP,TEMPB0,TEMPB1,TEMPB2,TEMPB3
GLOBAL LOOPCOUNT,AEXP,CARGB2
LSB equ 0
MSB equ 7
math_data UDATA
AARGB0 RES 1
AARGB1 RES 1
AARGB2 RES 1
AARGB3 RES 1
BARGB0 RES 1
BARGB1 RES 1
BARGB2 RES 1
BARGB3 RES 1
REMB0 RES 1
REMB1 RES 1
REMB2 RES 1
REMB3 RES 1
TEMP RES 1
TEMPB0 RES 1
TEMPB1 RES 1
TEMPB2 RES 1
TEMPB3 RES 1
ZARGB0 RES 1
ZARGB1 RES 1
ZARGB2 RES 1
CARGB2 RES 1
AEXP RES 1
LOOPCOUNT RES 1
math_code CODE
;---------------------------------------------------------------------
; 24-BIT ADDITION
_24_BitAdd
GLOBAL _24_BitAdd
movf BARGB2,w
addwf AARGB2,f
movf BARGB1,w
btfsc _C
incfsz BARGB1,w
addwf AARGB1,f
movf BARGB0,w
btfsc _C
incfsz BARGB0,w
addwf AARGB0,f
return
;---------------------------------------------------------------------
; 24-BIT SUBTRACTION
_24_bit_sub
GLOBAL _24_bit_sub
movf BARGB2,w
subwf AARGB2,f
movf BARGB1,w
btfss STATUS,C
incfsz BARGB1,w
subwf AARGB1,f
movf BARGB0,w
btfss STATUS,C
incfsz BARGB0,w
subwf AARGB0,f
return
;-------------------------------------------------------------------------
; 16x16 Bit Unsigned Fixed Point Multiply 16 x 16 -> 32
FXM1616U
GLOBAL FXM1616U
MOVFF AARGB1,TEMPB1
MOVF AARGB1,W
MULWF BARGB1
MOVFF PRODH,AARGB2
MOVFF PRODL,AARGB3
MOVF AARGB0,W
MULWF BARGB0
MOVFF PRODH,AARGB0
MOVFF PRODL,AARGB1
MULWF BARGB1
MOVF PRODL,W
ADDWF AARGB2,F
MOVF PRODH,W
ADDWFC AARGB1,F
CLRF WREG
ADDWFC AARGB0,F
MOVF TEMPB1,W
MULWF BARGB0
MOVF PRODL,W
ADDWF AARGB2,F
MOVF PRODH,W
ADDWFC AARGB1,F
CLRF WREG
ADDWFC AARGB0,F
RETLW 0x00
;--------------------------------------------------------------------
FXD2416U
GLOBAL FXD2416U
CLRF REMB0
CLRF REMB1
CLRF WREG
TSTFSZ BARGB0
GOTO D2416BGT1
MOVFF BARGB1,BARGB0
CALL FXD2408U
MOVFF REMB0,REMB1
CLRF REMB0
RETLW 0x00
D2416BGT1
CPFSEQ AARGB0
GOTO D2416AGTB
MOVFF AARGB1,AARGB0
MOVFF AARGB2,AARGB1
CALL FXD1616U
MOVFF AARGB1,AARGB2
MOVFF AARGB0,AARGB1
CLRF AARGB0
RETLW 0x00
D2416AGTB
MOVFF AARGB2,AARGB3
MOVFF AARGB1,AARGB2
MOVFF AARGB0,AARGB1
CLRF AARGB0
MOVFF AARGB0,TEMPB0
MOVFF AARGB1,TEMPB1
MOVFF AARGB2,TEMPB2
MOVFF AARGB3,TEMPB3
MOVLW 0x02 ; set loop count
MOVWF AEXP
MOVLW 0x01
MOVWF ZARGB0
BTFSC BARGB0,MSB
GOTO D2416UNRMOK
CALL DGETNRMD ; get normalization factor
MOVWF ZARGB0
MULWF BARGB1
MOVF BARGB0,W
MOVFF PRODL,BARGB1
MOVFF PRODH,BARGB0
MULWF ZARGB0
MOVF PRODL,W
ADDWF BARGB0,F
MOVF ZARGB0,W
MULWF AARGB3
MOVFF PRODL,TEMPB3
MOVFF PRODH,TEMPB2
MULWF AARGB1
MOVFF PRODL,TEMPB1
MOVFF PRODH,TEMPB0
MULWF AARGB2
MOVF PRODL,W
ADDWF TEMPB2,F
MOVF PRODH,W
ADDWF TEMPB1,F
D2416UNRMOK
BCF _C
CLRF TBLPTRH
RLCF BARGB0,W
RLCF TBLPTRH,F
ADDLW LOW (IBXTBL256+1) ; access reciprocal table
MOVWF TBLPTRL
MOVLW HIGH (IBXTBL256)
ADDWFC TBLPTRH,F
TBLRD *-
D2416ULOOP
MOVFF TEMPB0,AARGB0
MOVFF TEMPB1,AARGB1
CALL FXD1608U2 ; estimate quotient digit
BTFSS AARGB0,LSB
GOTO D2416UQTEST
SETF AARGB1
MOVFF TEMPB1,REMB0
MOVF BARGB0,W
ADDWF REMB0,F
BTFSC _C
GOTO D2416UQOK
D2416UQTEST
MOVF AARGB1,W ; test
MULWF BARGB1
MOVF PRODL,W
SUBWF TEMPB2,W
MOVF PRODH,W
SUBWFB REMB0,W
BTFSC _C
GOTO D2416UQOK
DECF AARGB1,F
MOVF BARGB0,W
ADDWF REMB0,F
BTFSC _C
GOTO D2416UQOK
MOVF AARGB1,W
MULWF BARGB1
MOVF PRODL,W
SUBWF TEMPB2,W
MOVF PRODH,W
SUBWFB REMB0,W
BTFSS _C
DECF AARGB1,F
D2416UQOK
MOVFF AARGB1,ZARGB1
MOVF AARGB1,W
MULWF BARGB1
MOVF PRODL,W
SUBWF TEMPB2,F
MOVF PRODH,W
SUBWFB TEMPB1,F
MOVF AARGB1,W
MULWF BARGB0
MOVF PRODL,W
SUBWF TEMPB1,F
MOVF PRODH,W
SUBWFB TEMPB0,F
BTFSS TEMPB0,MSB ; test
GOTO D2416QOK
DECF ZARGB1,F
MOVF BARGB1,W
ADDWF TEMPB2,F
MOVF BARGB0,W
ADDWFC TEMPB1,F
D2416QOK
DCFSNZ AEXP,F ; is loop done?
GOTO D2416FIXREM
MOVFF ZARGB1,ZARGB2
MOVFF TEMPB1,TEMPB0
MOVFF TEMPB2,TEMPB1
MOVFF TEMPB3,TEMPB2
GOTO D2416ULOOP
D2416FIXREM
MOVFF TEMPB1,REMB0
MOVFF TEMPB2,REMB1
MOVLW 0x01
CPFSGT ZARGB0
GOTO D2416REMOK
RRNCF ZARGB0,W
MOVWF BARGB0
CALL DGETNRMD
MULWF TEMPB2
MOVFF PRODH,REMB1
MULWF TEMPB1
MOVF PRODL,W
ADDWF REMB1,F
MOVFF PRODH,REMB0
D2416REMOK
CLRF AARGB0
MOVFF ZARGB1,AARGB2
MOVFF ZARGB2,AARGB1
RETLW 0x00
;----------------------------------------------------
FXD2408U
MOVFF AARGB0,TEMPB0
MOVFF AARGB1,TEMPB1
MOVFF AARGB2,TEMPB2
CALL FXD1608U
MOVFF AARGB0,TEMPB0
MOVFF AARGB1,TEMPB1
MOVFF TEMPB2,AARGB1
MOVFF REMB0,AARGB0
CALL FXD1608U
MOVFF AARGB1,AARGB2
MOVFF TEMPB1,AARGB1
MOVFF TEMPB0,AARGB0
RETLW 0x00
;--------------------------------------------------------
FXD1608U
GLOBAL FXD1608U
MOVLW 0x01
CPFSGT BARGB0
GOTO DREMZERO8
FXD1608U1
GLOBAL FXD1608U1
BCF _C
CLRF TBLPTRH
RLCF BARGB0,W
RLCF TBLPTRH,F
ADDLW LOW (IBXTBL256+1) ; access reciprocal table
MOVWF TBLPTRL
MOVLW HIGH (IBXTBL256)
ADDWFC TBLPTRH,F
TBLRD *-
FXD1608U2
GLOBAL FXD1608U2
MOVFF AARGB0,REMB1
MOVFF AARGB1,REMB0
MOVF TABLAT,W ; estimate quotient
MULWF REMB1
MOVFF PRODH,AARGB0
MOVFF PRODL,AARGB1
TBLRD *+
MOVF TABLAT,W
MULWF REMB0
MOVFF PRODH,AARGB2
MULWF REMB1
MOVF PRODL,W
ADDWF AARGB2,F
MOVF PRODH,W
ADDWFC AARGB1,F
CLRF WREG
ADDWFC AARGB0,F
TBLRD *-
MOVF TABLAT,W
MULWF REMB0
MOVF PRODL,W
ADDWF AARGB2,F
MOVF PRODH,W
ADDWFC AARGB1,F
CLRF WREG
ADDWFC AARGB0,F
MOVF BARGB0,W
MULWF AARGB1
MOVFF PRODL,AARGB3
MOVFF PRODH,AARGB2
MULWF AARGB0
MOVF PRODL,W
ADDWF AARGB2,F
MOVF AARGB3,W ; estimate remainder
SUBWF REMB0,F
MOVF AARGB2,W
SUBWFB REMB1,F
BTFSS REMB1,MSB ; test remainder
RETLW 0x00
DECF AARGB1,F
CLRF WREG
SUBWFB AARGB0,F
MOVF BARGB0,W
ADDWF REMB0,F
RETLW 0x00
;----------------------------------------------------------
FXD1616U
TSTFSZ BARGB0
GOTO D1616B0GT0
MOVFF BARGB1,BARGB0
CALL FXD1608U
MOVFF REMB0,REMB1
CLRF REMB0
RETLW 0x00
D1616B0GT0
MOVF BARGB0,W
SUBWF AARGB0,W
BTFSS _C
GOTO D1616QZERO
BTFSS _Z
GOTO D1616AGEB
MOVF BARGB1,W
SUBWF AARGB1,W
BTFSS _C
GOTO D1616QZERO
D1616AGEB
MOVFF AARGB0,TEMPB0
MOVFF AARGB1,TEMPB1
MOVFF AARGB1,CARGB2
MOVFF AARGB0,AARGB1
CLRF AARGB0
MOVFF BARGB0,BARGB2
MOVFF BARGB1,BARGB3
BTFSC BARGB0,MSB
GOTO D1616UNRMOK
MOVF BARGB0,W
RLNCF WREG,F
ADDLW LOW (IBXTBL256+3) ; access reciprocal table
MOVWF TBLPTRL
MOVLW HIGH (IBXTBL256)
CLRF TBLPTRH
ADDWFC TBLPTRH,F
TBLRD *
MOVF TABLAT,W ; normalize
MULWF BARGB3
MOVFF PRODL,BARGB1
MOVFF PRODH,BARGB0
MULWF BARGB2
MOVF PRODL,W
ADDWF BARGB0,F
MOVF TABLAT,W
MULWF TEMPB1
MOVFF PRODL,CARGB2
MOVFF PRODH,AARGB1
MULWF TEMPB0
MOVF PRODL,W
ADDWF AARGB1,F
CLRF AARGB0
MOVF PRODH,W
ADDWFC AARGB0,F
D1616UNRMOK
CALL FXD1608U1 ; estimate quotient digit
MOVF AARGB1,W
MULWF BARGB1
MOVF PRODL,W
SUBWF CARGB2,W
MOVF PRODH,W
SUBWFB REMB0,W
BTFSS _C ; test
DECF AARGB1,F
D1616UQOK
MOVF AARGB1,W ; calculate remainder
MULWF BARGB3
MOVF PRODL,W
SUBWF TEMPB1,F
MOVF PRODH,W
SUBWFB TEMPB0,F
MOVF AARGB1,W
MULWF BARGB2
MOVF PRODL,W
SUBWF TEMPB0,F
; This test does not appear to be necessary in the 16 bit case, but
; is included here in the event that a case appears after testing.
; BTFSS TEMPB0,MSB ; test
; GOTO D1616QOK
; DECF AARGB1
; MOVF BARGB3,W
; ADDWF TEMPB1
; MOVF BARGB2,W
; ADDWFC TEMPB0
D1616QOK
MOVFF TEMPB0,REMB0
MOVFF TEMPB1,REMB1
RETLW 0x00
;---------------------------------------------------------
DGETNRMD
MOVLW 0x10
CPFSLT BARGB0
GOTO DGETNRMDH
DGETNRMDL
BTFSC BARGB0,3
RETLW 0x10
BTFSC BARGB0,2
RETLW 0x20
BTFSC BARGB0,1
RETLW 0x40
BTFSC BARGB0,0
RETLW 0x80
DGETNRMDH
BTFSC BARGB0,6
RETLW 0x02
BTFSC BARGB0,5
RETLW 0x04
BTFSC BARGB0,4
RETLW 0x08
;----------------------------------------------------------------------------------------------
; Routines for the trivial cases when the quotient is zero.
; Timing: 9,7,5 clks
; PM: 9,7,5 DM: 8,6,4
;D3232QZERO
; MOVFF AARGB3,REMB3
; CLRF AARGB3
;D2424QZERO
; MOVFF AARGB2,REMB2
; CLRF AARGB2
D1616QZERO
MOVFF AARGB1,REMB1
CLRF AARGB1
MOVFF AARGB0,REMB0
CLRF AARGB0
RETLW 0x00
DREMZERO8
CLRF REMB0
RETLW 0x00
;----------------------------------------------------------------------------------------------
; The table IBXTBL256 is used by all routines and consists of 16-bit
; upper bound approximations to the reciprocal of BARGB0.
IBXTBL256
GLOBAL IBXTBL256
DATA 0x0000
DATA 0x0001
DATA 0x8001
DATA 0x5556
DATA 0x4001
DATA 0x3334
DATA 0x2AAB
DATA 0x2493
DATA 0x2001
DATA 0x1C72
DATA 0x199A
DATA 0x1746
DATA 0x1556
DATA 0x13B2
DATA 0x124A
DATA 0x1112
DATA 0x1001
DATA 0x0F10
DATA 0x0E39
DATA 0x0D7A
DATA 0x0CCD
DATA 0x0C31
DATA 0x0BA3
DATA 0x0B22
DATA 0x0AAB
DATA 0x0A3E
DATA 0x09D9
DATA 0x097C
DATA 0x0925
DATA 0x08D4
DATA 0x0889
DATA 0x0843
DATA 0x0801
DATA 0x07C2
DATA 0x0788
DATA 0x0751
DATA 0x071D
DATA 0x06EC
DATA 0x06BD
DATA 0x0691
DATA 0x0667
DATA 0x063F
DATA 0x0619
DATA 0x05F5
DATA 0x05D2
DATA 0x05B1
DATA 0x0591
DATA 0x0573
DATA 0x0556
DATA 0x053A
DATA 0x051F
DATA 0x0506
DATA 0x04ED
DATA 0x04D5
DATA 0x04BE
DATA 0x04A8
DATA 0x0493
DATA 0x047E
DATA 0x046A
DATA 0x0457
DATA 0x0445
DATA 0x0433
DATA 0x0422
DATA 0x0411
DATA 0x0401
DATA 0x03F1
DATA 0x03E1
DATA 0x03D3
DATA 0x03C4
DATA 0x03B6
DATA 0x03A9
DATA 0x039C
DATA 0x038F
DATA 0x0382
DATA 0x0376
DATA 0x036A
DATA 0x035F
DATA 0x0354
DATA 0x0349
DATA 0x033E
DATA 0x0334
DATA 0x032A
DATA 0x0320
DATA 0x0316
DATA 0x030D
DATA 0x0304
DATA 0x02FB
DATA 0x02F2
DATA 0x02E9
DATA 0x02E1
DATA 0x02D9
DATA 0x02D1
DATA 0x02C9
DATA 0x02C1
DATA 0x02BA
DATA 0x02B2
DATA 0x02AB
DATA 0x02A4
DATA 0x029D
DATA 0x0296
DATA 0x0290
DATA 0x0289
DATA 0x0283
DATA 0x027D
DATA 0x0277
DATA 0x0271
DATA 0x026B
DATA 0x0265
DATA 0x025F
DATA 0x025A
DATA 0x0254
DATA 0x024F
DATA 0x024A
DATA 0x0244
DATA 0x023F
DATA 0x023A
DATA 0x0235
DATA 0x0231
DATA 0x022C
DATA 0x0227
DATA 0x0223
DATA 0x021E
DATA 0x021A
DATA 0x0215
DATA 0x0211
DATA 0x020D
DATA 0x0209
DATA 0x0205
DATA 0x0201
DATA 0x01FD
DATA 0x01F9
DATA 0x01F5
DATA 0x01F1
DATA 0x01ED
DATA 0x01EA
DATA 0x01E6
DATA 0x01E2
DATA 0x01DF
DATA 0x01DB
DATA 0x01D8
DATA 0x01D5
DATA 0x01D1
DATA 0x01CE
DATA 0x01CB
DATA 0x01C8
DATA 0x01C4
DATA 0x01C1
DATA 0x01BE
DATA 0x01BB
DATA 0x01B8
DATA 0x01B5
DATA 0x01B3
DATA 0x01B0
DATA 0x01AD
DATA 0x01AA
DATA 0x01A7
DATA 0x01A5
DATA 0x01A2
DATA 0x019F
DATA 0x019D
DATA 0x019A
DATA 0x0198
DATA 0x0195
DATA 0x0193
DATA 0x0190
DATA 0x018E
DATA 0x018B
DATA 0x0189
DATA 0x0187
DATA 0x0184
DATA 0x0182
DATA 0x0180
DATA 0x017E
DATA 0x017B
DATA 0x0179
DATA 0x0177
DATA 0x0175
DATA 0x0173
DATA 0x0171
DATA 0x016F
DATA 0x016D
DATA 0x016B
DATA 0x0169
DATA 0x0167
DATA 0x0165
DATA 0x0163
DATA 0x0161
DATA 0x015F
DATA 0x015D
DATA 0x015B
DATA 0x0159
DATA 0x0158
DATA 0x0156
DATA 0x0154
DATA 0x0152
DATA 0x0151
DATA 0x014F
DATA 0x014D
DATA 0x014B
DATA 0x014A
DATA 0x0148
DATA 0x0147
DATA 0x0145
DATA 0x0143
DATA 0x0142
DATA 0x0140
DATA 0x013F
DATA 0x013D
DATA 0x013C
DATA 0x013A
DATA 0x0139
DATA 0x0137
DATA 0x0136
DATA 0x0134
DATA 0x0133
DATA 0x0131
DATA 0x0130
DATA 0x012F
DATA 0x012D
DATA 0x012C
DATA 0x012A
DATA 0x0129
DATA 0x0128
DATA 0x0126
DATA 0x0125
DATA 0x0124
DATA 0x0122
DATA 0x0121
DATA 0x0120
DATA 0x011F
DATA 0x011D
DATA 0x011C
DATA 0x011B
DATA 0x011A
DATA 0x0119
DATA 0x0117
DATA 0x0116
DATA 0x0115
DATA 0x0114
DATA 0x0113
DATA 0x0112
DATA 0x0110
DATA 0x010F
DATA 0x010E
DATA 0x010D
DATA 0x010C
DATA 0x010B
DATA 0x010A
DATA 0x0109
DATA 0x0108
DATA 0x0107
DATA 0x0106
DATA 0x0105
DATA 0x0104
DATA 0x0103
DATA 0x0102
DATA 0x0101
end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -