math.inc

keil下开发的永磁同步电机矢量控制程序

;****************************************************************************************
; Notation
; 	(a,b) in Q31 format : a represent the high part of the number
; 	                      b represent the low part of the number

;****************************************************************************************

;****************************************************************************************
;			MACRO DEFINITION
;****************************************************************************************

;---------------------------------
; ADDS_Q15 a,b
;_________________________________
; add and saturate two Q15 number 
; a, b are in Q15 format
; a = a + b
; modified : a
%*DEFINE (ADDS_Q15(a, b)) LOCAL NoSaturation,NegativeSaturation,PositiveSaturation (
	ADD 	%a,%b
	JMPR	cc_NV,%NoSaturation
;Saturate
	JMPR	cc_NN,%NegativeSaturation		; flag N not set => negative overflow	
%PositiveSaturation:
	MOV 	%a,#0x7FFF
	JMPR	cc_UC,%NoSaturation
%NegativeSaturation:		; flag N set => positive overflow	
	MOV 	%a,#0x8000
%NoSaturation:
)


;---------------------------------
; ADDS_Q31 a,b,c,d
;_________________________________
; add and saturate two Q31 number 
; (a, b) and (c,d) are in Q31 format
; (a,b) = (a,b) + (c,d)
; modified : a, b
%*DEFINE (ADDS_Q31(a ,b ,c ,d)) LOCAL NoSaturation,NegativeSaturation,PositiveSaturation (
	ADD 	%b,%d
	ADDC 	%a,%c
	JMPR	cc_NV,%NoSaturation
;Saturate
	JMPR	cc_NN,%NegativeSaturation		; flag N not set => negative overflow	
%PositiveSaturation:
	MOV 	%a,#0x7FFF
	MOV 	%b,ONES
	JMPR	cc_UC,%NoSaturation
%NegativeSaturation:					; flag N set => positive overflow	
	MOV 	%a,#0x8000
	MOV 	%b,ZEROS
%NoSaturation:
)


;---------------------------------
; SUBS_Q15 a,b
;_________________________________
; substract and saturate two Q15 number
; a, b are in Q15 format
; a = a - b
; modified : a
%*DEFINE (SUBS_Q15(a, b)) LOCAL NoSaturation,NegativeSaturation,PositiveSaturation (
	SUB 	%a,%b
	JMPR	cc_NV,%NoSaturation
;Saturate
	JMPR	cc_NN,%NegativeSaturation		; flag N not set => negative overflow	
%PositiveSaturation:
	MOV 	%a,#0x7FFF
	JMPR	cc_UC,%NoSaturation
%NegativeSaturation:		; flag N set => positive overflow	
	MOV 	%a,#0x8000
%NoSaturation:
)

;---------------------------------
; SUBS_Q31 a,b,c,d
;_________________________________
; substract and saturate two Q31 number
; (a, b) and  (c,d) are in Q31 format
; (a,b) = (a,b) - (c,d)
; modified : a
%*DEFINE (SUBS_Q31(a, b,c,d)) LOCAL NoSaturation,NegativeSaturation,PositiveSaturation (
	SUB 	%b,%d
	SUBC 	%a,%c
	JMPR	cc_NV,%NoSaturation
;Saturate
	JMPR	cc_NN,%NegativeSaturation		; flag N not set => negative overflow	
%PositiveSaturation:
	MOV 	%a,#0x7FFF
	MOV 	%a,ONES
	JMPR	cc_UC,%NoSaturation
%NegativeSaturation:		; flag N set => positive overflow	
	MOV 	%a,#0x8000
	MOV 	%a,ZEROS
%NoSaturation:
)
;---------------------------------
; MULS_Q15 a,b,c
;_________________________________
; multiply and saturate two Q15 number and shift one by left 
; a, b are in Q15 format
; c = a * b	result in Q15
; Note: -1*-1= +1
; modified : a,c 
%*DEFINE (MULS_Q15(a, b, c)) LOCAL NotPlusOne,EndMultiply(
	MOV	%a,%a
	JNB	PSW.4,%NotPlusOne
	MOV	%b,%b
	JNB	PSW.4,%NotPlusOne
	MOV	%c,#0x7FFF
	JMPR	cc_UC,%EndMultiply
%NotPlusOne:
	MUL	%a,%b
	MOV	%c,MDH
	SHL	%c,#1
	MOV	%a,MDL
	SHR	%a,#15
	OR	%c,%a
%EndMultiply:
)



;---------------------------------
; MULS_Q31 a,b
;_________________________________
; multiply and saturate two Q15 number and shift one by left 
; a, b are in Q15 format
; (a,b) = a * b	result in Q31
; Note: -1*-1= +1
; modified : a,b, R1
; a,b  can't be R1
%*DEFINE (MULS_Q31(a, b)) LOCAL NotPlusOne,EndMultiply(
	MOV	%a,%a
	JNB	PSW.4,%NotPlusOne
	MOV	%b,%b
	JNB	PSW.4,%NotPlusOne
	MOV	%a,#0x7FFF
	MOV	%b,ONES
	JMPR	cc_UC,%EndMultiply
%NotPlusOne:
	MUL	%a,%b
	MOV	%a,MDH
	SHL	%a,#1
	MOV	R1,MDL
	MOV	%b,R1
	SHR	R1,#15
	OR	%a,R1
	SHL	%b,#1
%EndMultiply:
)


;---------------------------------
; LIMIT_32 a,b,c,d
;_________________________________
; limits the 32 bit signed number (a,b) by -c,c
; (a, b) are in Q31 format
; c is in Q15 and positiv
; result in Q15
; modified : a,b,c,R1,R2
; a,b,c,d  can't be R1 or R2
%*DEFINE (LIMIT_32(a, b, c, d)) LOCAL LimitNeg,LimitPos,EndLimit(
	MOV	%d,ZEROS		; c is positiv (full d with the signe bit of c)
	MOV 	R1,%a
	MOV 	R2,%b
	SUB	%b,%c
	SUBC	%a,%d
	JMPR	cc_SGE,%LimitPos
	NEG	%c
	MOV	%d,ONES		; c is negativ (full d with the signe bit of c)
	SUB	R2,%c
	SUBC	R1,%d
	JMPR	cc_SLE,%LimitNeg
	MOV	%d,%b
	JMPR	cc_UC,%EndLimit
%LimitNeg:
	MOV	%d,%c
	JMPR	cc_UC,%EndLimit
%LimitPos:
	MOV	%d,%c
%EndLimit:
)


;---------------------------------
; LIMIT_16 a,b,c
;_________________________________
; limits the 16 bit signed number a by -b,b
; b is a 16 bit signed number, positiv
; result in a
; modified : a,b,c
%*DEFINE (LIMIT_16(a, b, c)) LOCAL LimitNeg,LimitPos,EndLimit(
	MOV	%c,%a
	SUB	%c,%b
	JMPR	cc_SGE,%LimitPos
	NEG	%b
	MOV	%c,%a
	SUB	%c,%b
	JMPR	cc_SLE,%LimitNeg
	JMPR	cc_UC,%EndLimit
%LimitNeg:
	MOV	%a,%b
	JMPR	cc_UC,%EndLimit
%LimitPos:
	MOV	%a,%b
%EndLimit:
)



;---------------------------------
; MSUB_Q15(a, b, c, d, e)
;_________________________________
; a * b - c * d
; (a, c) and (b, d) are in Q15 format
; (e) result in Q15
; modified : e
%*DEFINE (MSUB_Q15(a, b, c, d, e))(
	MUL	%a,%b
	MOV	%e,MDH	 		; e = a*c	-1 < x < 1	in Q14
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	MUL	%c,%d
;					; MDH = b*d	-1 < x < 1	in Q14
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	SUB	%e,MDH			; e = a*c - b*d
; No verification is done for 0x4000 + 0x4000 = 0x7FFF, e and f are never equal to 0x4000 = -1 in Q14

	ADD	%e,%e
; No verification is done for a result > 1 or < -1; no saturation
)

;---------------------------------
; MADD_Q15(a, b, c, d, e)
;_________________________________
; a * c + b * d
; (a, c) and (b, d) are in Q15 format
; (e) result in Q15
; modified : e
%*DEFINE (MADD_Q15(a, b, c, d, e))(
	MUL	%a,%b
	MOV	%e,MDH	 		; e = a*c	-1 < x < 1	in Q14
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	MUL	%c,%d
;					; MDH = b*d	-1 < x < 1	in Q14
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	ADD	%e,MDH			; e = a*c + b*d
; No verification is done for 0x4000 + 0x4000 = 0x7FFF, e and f are never equal to 0x4000 = -1 in Q14

	ADD	%e,%e
; No verification is done for a result > 1 or < -1; no saturation
)


;---------------------------------
; MADDS_Q31(a, b, c, d, e, f)
;_________________________________
; (a,b) + c * d + e * f
; c, d, e, f are in Q15 format
; (a,b) result in Q2.30
; modified : a,b 
%*DEFINE (MADDS_Q31(a, b, c, d, e, f))(
	MUL	%c,%d
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	%ADDS_Q31(%a ,%b ,MDH ,MDL)

	MUL	%e,%f
; No verification is done for (-1)*(-1)
; at least one of the two operand must be bigger than 0x8001 and smaller 0x7FFF

	%ADDS_Q31(%a ,%b ,MDH ,MDL)
)

;---------------------------------
; LSHIFTS_Q31 a,b
;_________________________________
; Shift left by 4 and saturate a 32 bit signed number 
; (a, b) is a 32 bit signed number
; (a,b) = (a,b) << 4 and saturate
; modified : a, b
%*DEFINE (LSHIFTS_Q31(a ,b)) LOCAL Saturation,NoSaturation,NegativeSaturation,PositiveSaturation (
	ADD 	%b,%b
	ADDC 	%a,%a
	JMPR	cc_V,%Saturation
	ADD 	%b,%b
	ADDC 	%a,%a
	JMPR	cc_V,%Saturation
	ADD 	%b,%b
	ADDC 	%a,%a
	JMPR	cc_V,%Saturation
	ADD 	%b,%b
	ADDC 	%a,%a
	JMPR	cc_V,%Saturation
	JMPR	cc_UC,%NoSaturation
%Saturation:
	JMPR	cc_NN,%NegativeSaturation		; flag N not set => negative overflow	
%PositiveSaturation:
	MOV 	%a,#0x7FFF
	MOV 	%b,ONES
	JMPR	cc_UC,%NoSaturation
%NegativeSaturation:					; flag N set => positive overflow	
	MOV 	%a,#0x8000
	MOV 	%b,ZEROS
%NoSaturation:
)

;****************************************************************************************
;			END OF MACRO DEFINITION
;****************************************************************************************