SX Microcontroller Math Method

Complex number magnitude calculation

by Nikolai Golovchenko

;***********************************************
; Complex number magnitude calculation
; using CORDIC algorithm described at
; www.dspguru.com\info\faqs\cordic.htm
;
; Input:
;  ReH:ReL, ImH:ImL - complex number (16 bit signed)
;
; Output:
;  ReH:ReL - magnitude (16 bit unsigned)
;  ImH:ImL - garbage
;
; Temporaries:
;  RekH:RekL - Re multipled by k (k=2^-L, L=0,1,2,...15)
;  Counter - loop counter
;  Temp
;
; Instructions: 147
; Execution time(worst case including return):
;  18+18+15*(8+2+20+5+7.5*10-2)+60 ~= 1700 instruction cycles

; Notes:
;  1) Precision is 0.028%, depends on how exact
;  the division by CORDIC gain is implemented:
;	(0.60725293510314)
;	a) 1/2+1/8-1/64-1/512 -> 0.028%
;	b) 1/2+1/8-1/64-1/512-1/4096 -> 0.012384%
;	c) 1/2+1/8-1/64-1/512-1/4096+1/16384 -> 0.002333%
;  2) Range of input data should be restricted so
;  that M=sqrt(Re*Re+Im*Im) is less than 65536*0.60725293510314~=39760
;  to prevent overflow in magnitude during calculation
;  3) To reduce execution time, the number of loops can be
;  reduced to 8. The angle after rotation the initial
;  vector 8 times is less then 0.22381 deg, which is good
;  enough precision. Besides, the gain at 8 rotations is smaller
;  and closer to the approximated gain, which is used in this code.
;  Reduced execution time will be ~850 cycles!
;
; 6 Aug 2000 by Nikolai Golovchenko
;***********************************************
Magnitude16
;Find absolute value of the vector components
	sb	ReH.7		;Re = abs(Re)
	jmp	Magnitude16a
	not	ReL
	not	ReH
	inc	ReL
	snb	Z
	inc	ReH
Magnitude16a
	sb	ImH.7		;Im = abs(Im)
	jmp	Magnitude16b
	not	ImL
	not	ImH
	inc	ImL
	snb	Z
	inc	ImH
Magnitude16b
;Test imaginary part for zero and if yes, quit
	mov	W, ImL
	or	W, ImH
	snb	Z
	ret	;quit if zero imaginary part
;Perform first iteration
	mov	W, ImL		;Imk = Im
	mov	ImkL, W
	mov	W, ImH
	mov	ImkH, W

	mov	W, ReL		;Im' = Im - Re
	sub	ImL, W
	mov	W, ReH
	sb	C
	movsz	W, ++ReH
	sub	ImH, W

	mov	W, ImkL		;Re' = Re + Im = Re + Imk
	add	ReL, W
	mov	W, ImkH
	snb	C
	movsz	W, ++ImkH
	add	ReH, W
;Begin loop
	mov	W, #1
	mov	Counter, W
Magnitude16loop
;load scaled values
	mov	W, ImL		;Imk = Im
	mov	ImkL, W
	mov	W, ImH
	mov	ImkH, W
	mov	W, ReL		;Rek = Re
	mov	RekL, W
	mov	W, ReH
	mov	RekH, W
;scale them (1 to 15 right shifts)
	mov	W, Counter	;load counter value to Temp
	mov	Temp, W
Magnitude16loop2
	clrb	C		;unsigned right shift for Rek
	rr	RekH
	rr	RekL
	mov	W, <<ImkH	;signed right shift for Imk
	rr	ImkH
	rr	ImkL
	decsz	Temp
	jmp	Magnitude16loop2
;update current values
	mov	W, ImkL
	snb	ImH.7		;if Im < 0 add a phase, if Im >= 0 substract a phase
	jmp	Magnitude16AddPhase
;substract a phase
	add	ReL, W		;Re' = Re + Imk
	mov	W, ImkH
	snb	C
	movsz	W, ++ImkH
	add	ReH, W

	mov	W, RekL		;Im' = Im - Rek
	sub	ImL, W
	mov	W, RekH
	sb	C
	movsz	W, ++RekH
	sub	ImH, W

	jmp	Magnitude16loopend
Magnitude16AddPhase
;add a phase
	snb	C		;correct Imk, because shifts of negative
	movsz	W, ++ImkL	;values like (-1 >> 1 = -1) can
	dec	ImkH		;accumulate error. With this correction,
	inc	ImkH		;shifts of negative values will work like
				;shifts of positive values (i.e. round to zero)

	sub	ReL, W		;Re' = Re - Imk
	mov	W, ImkH
	sb	C
	movsz	W, ++ImkH
	sub	ReH, W

	mov	W, RekL		;Im' = Im + Rek
	add	ImL, W
	mov	W, RekH
	snb	C
	movsz	W, ++RekH
	add	ImH, W
Magnitude16loopend
	inc	Counter
	sb	Counter.4	;repeat untill counter reaches 16
;or uncomment this for better performance
;	sb	Counter.3	;repeat untill counter reaches 8
	jmp	Magnitude16loop

;Optional:
;Divide result by 1.64676025786545 (CORDIC gain)
;or multiply by 0.60725293510314 = 1/2+1/8-1/64-1/512 - 0.028%
	mov	W, ReH
	mov	RekH, W
	mov	W, ReL
	mov	RekL, W
	clrb	C
	rr	ReH
	rr	ReL
	clrb	C
	rr	ReH
	rr	ReL
	clrb	C
	rr	ReH
	rr	ReL
	not	ReL
	not	ReH
	inc	ReL
	snb	Z
	inc	ReH
	clr	Temp
	snb	ReH.7
	not	Temp
	sub	ReL, W
	mov	W, RekH
	sb	C
	movsz	W, ++RekH
	sub	ReH, W
	sb	C
	dec	Temp
	rr	Temp
	rr	ReH
	rr	ReL
	rr	Temp
	rr	ReH
	rr	ReL
	mov	W, <<ReH
	rr	ReH
	rr	ReL
	mov	W, RekL
	add	ReL, W
	mov	W, RekH
	snb	C
	movsz	W, ++RekH
	add	ReH, W
	clrb	C
	rr	ReH
	rr	ReL
	clrb	C
	rr	ReH
	rr	ReL
	mov	W, RekL
	add	ReL, W
	mov	W, RekH
	snb	C
	movsz	W, ++RekH
	add	ReH, W
	rr	ReH
	rr	ReL

;Done!
	ret
;***********************************************