Complex number magnitude calculation
Nikolai Golovchenko says:
Nikolai Golovchenko says:
| CODE |
cblock 0x20 Temp, Counter ReH, ReL, ImH, ImL RekH, RekL, ImkH, ImkL endc #define XX 22520 #define YY 1 movlw low(XX) movwf ReL movlw high(XX) movwf ReH movlw low(YY) movwf ImL movlw high(YY) movwf ImH again call Magnitude16 stop nop incf ReL, f skpnz incf ReH, f movf ImL, f skpnz decf ImH, f decf ImL, f goto again ;*********************************************** ; 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+4+7.5*9)+60 ~= 1600 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 ~800 cycles! ; ; 6 Aug 2000 by Nikolai Golovchenko ;*********************************************** Magnitude16 ;Find absolute value of the vector components btfss ReH, 7 ;Re = abs(Re) goto Magnitude16a comf ReL, f comf ReH, f incf ReL, f skpnz incf ReH, f Magnitude16a btfss ImH, 7 ;Im = abs(Im) goto Magnitude16b comf ImL, f comf ImH, f incf ImL, f skpnz incf ImH, f Magnitude16b ;Test imaginary part for zero and if yes, quit movf ImL, w iorwf ImH, w skpnz return ;quit if zero imaginary part ;Perform first iteration movf ImL, w ;Imk = Im movwf ImkL movf ImH, w movwf ImkH movf ReL, w ;Im' = Im - Re subwf ImL, f movf ReH, w skpc incfsz ReH, w subwf ImH, f movf ImkL, w ;Re' = Re + Im = Re + Imk addwf ReL, f movf ImkH, w skpnc incfsz ImkH, w addwf ReH, f ;Begin loop movlw 1 movwf Counter Magnitude16loop ;load scaled values movf ImL, w ;Imk = Im movwf ImkL movf ImH, w movwf ImkH movf ReL, w ;Rek = Re movwf RekL movf ReH, w movwf RekH ;scale them (1 to 15 right shifts) movf Counter, w ;load counter value to Temp movwf Temp Magnitude16loop2 clrc ;unsigned right shift for Rek rrf RekH, f rrf RekL, f rlf ImkH, w ;signed right shift for Imk rrf ImkH, f rrf ImkL, f decfsz Temp, f goto Magnitude16loop2 ;update current values movf ImkL, w btfsc ImH, 7 ;if Im < 0 add a phase, if Im >= 0 substract a phase goto Magnitude16AddPhase ;substract a phase addwf ReL, f ;Re' = Re + Imk movf ImkH, w skpnc incfsz ImkH, w addwf ReH, f movf RekL, w ;Im' = Im - Rek subwf ImL, f movf RekH, w skpc incfsz RekH, w subwf ImH, f goto Magnitude16loopend Magnitude16AddPhase ;add a phase skpnc ;correct Imk, because shifts of negative incfsz ImkL, w ;values like (-1 >> 1 = -1) can decf ImkH, f ;accumulate error. With this correction, incf ImkH, f ;shifts of negative values will work like ;shifts of positive values (i.e. round to zero) subwf ReL, f ;Re' = Re - Imk movf ImkH, w skpc incfsz ImkH, w subwf ReH, f movf RekL, w ;Im' = Im + Rek addwf ImL, f movf RekH, w skpnc incfsz RekH, w addwf ImH, f Magnitude16loopend incf Counter, f btfss Counter, 4;repeat untill counter reaches 16 ;or uncomment this for better performance ; btfss Counter, 3;repeat untill counter reaches 8 goto Magnitude16loop ;Optional: ;Divide result by 1.64676025786545 (CORDIC gain) ;or multiply by 0.60725293510314 = 1/2+1/8-1/64-1/512 - 0.028% movf ReH, w movwf RekH movf ReL, w movwf RekL clrc rrf ReH, f rrf ReL, f clrc rrf ReH, f rrf ReL, f clrc rrf ReH, f rrf ReL, f comf ReL, f comf ReH, f incf ReL, f skpnz incf ReH, f clrf Temp btfsc ReH, 7 comf Temp, f subwf ReL, f movf RekH, w skpc incfsz RekH, w subwf ReH, f skpc decf Temp, f rrf Temp, f rrf ReH, f rrf ReL, f rrf Temp, f rrf ReH, f rrf ReL, f rlf ReH, w rrf ReH, f rrf ReL, f movf RekL, w addwf ReL, f movf RekH, w skpnc incfsz RekH, w addwf ReH, f clrc rrf ReH, f rrf ReL, f clrc rrf ReH, f rrf ReL, f movf RekL, w addwf ReL, f movf RekH, w skpnc incfsz RekH, w addwf ReH, f rrf ReH, f rrf ReL, f ;Done! return |