\ -*- coding: utf-8 -*-
-
-; ----------
-; CORDIC.f
-; ----------
\ see CORDICforDummies.pdf
\
-; -----------------------------------------------------------
-; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
-; -----------------------------------------------------------
-\
\ to see kernel options, download FastForthSpecs.f
\ FastForth kernel options: MSP430ASSEMBLER, CONDCOMP, FIXPOINT_INPUT
\
\ TARGET SELECTION ( = the name of \INC\target.pat file without the extension)
\ LP_MSP430FR2476
\ MSP_EXP430FR5739 MSP_EXP430FR5969 MSP_EXP430FR5994 MSP_EXP430FR6989
-\ MSP_EXP430FR2433 MSP_EXP430FR2355 CHIPSTICK_FR2433
+\ MSP_EXP430FR2433 CHIPSTICK_FR2433 MSP_EXP430FR2355
\
\ from scite editor : copy your target selection in (shift+F8) parameter 1:
\
\ ASSEMBLER conditionnal usage with IF UNTIL WHILE S< S>= U< U>= 0= 0<> 0>=
\ ASSEMBLER conditionnal usage with ?JMP ?GOTO S< S>= U< U>= 0= 0<> 0<
-PWR_STATE
+CODE ABORT_CORDIC
+SUB #4,PSP
+MOV TOS,2(PSP)
+MOV &KERNEL_ADDON,TOS
+BIT #BIT10,TOS
+0<> IF MOV #0,TOS THEN \ if TOS <> 0 (FIXPOINT input), set TOS = 0
+MOV TOS,0(PSP)
+MOV &VERSION,TOS
+SUB #307,TOS \ FastForth V3.7
+COLON
+$0D EMIT \ return to column 1 without CR
+ABORT" FastForth version = 3.7 please!"
+ABORT" build FastForth with FIXPOINT_INPUT addon !"
+PWR_STATE \ if no abort remove this word
+;
+
+ABORT_CORDIC
+
+; ----------
+; CORDIC.f
+; ----------
[DEFINED] {CORDIC} [IF] {CORDIC} [THEN]
\
CREATE T_ARCTAN \ ArcTan table
-11520 , \ 256 * 45
-6801 , \ 256 * 26.565
-3593 , \ 256 * 14.036
-1824 , \ 256 * 7.125
-916 , \ 256 * 3.576
-458 , \ 256 * 1.790
-229 , \ 256 * 0.895
-115 , \ 256 * 0.448
-57 , \ 256 * 0.224
-29 , \ 256 * 0.112
-14 , \ 256 * 0.056
-7 , \ 256 * 0.028
-4 , \ 256 * 0.014
-2 , \ 256 * 0.007
-1 , \ 256 * 0.003
+12870 , \ 286 * 45 =
+7598 , \ 286 * 26.565 = 7597,605
+4014 , \ 286 * 14.036 = 4014,366
+2038 , \ 286 * 7.125 = 2037,755
+1023 , \ 286 * 3.576 = 1022,832
+512 , \ 286 * 1.790 = 511,914
+256 , \ 286 * 0.895 = 256,020
+128 , \ 286 * 0.448 = 128,017
+64 , \ 286 * 0.224 = 64,010
+32 , \ 286 * 0.112 = 32,005
+16 , \ 286 * 0.056 = 16,0025
+8 , \ 286 * 0.028 = 8,00126
+4 , \ 286 * 0.014 = 4
+2 , \ 286 * 0.007 = 2
+1 , \ 286 * 0.003 = 1
CREATE T_SCALE \ 1/Gi table
46340 , \ = 65536 * cos(45)
39797 ,
39797 ,
-[UNDEFINED] @ [IF]
-\ https://forth-standard.org/standard/core/Fetch
-\ @ c-addr -- char fetch char from memory
-CODE @
-MOV @TOS,TOS
-MOV @IP+,PC
-ENDCODE
-[THEN]
-
-[UNDEFINED] R> [IF]
-\ https://forth-standard.org/standard/core/Rfrom
-\ R> -- x R: x -- pop from return stack ; CALL #RFROM performs DOVAR
-CODE R>
-SUB #2,PSP \ 1
-MOV TOS,0(PSP) \ 3
-MOV @RSP+,TOS \ 2
-MOV @IP+,PC \ 4
-ENDCODE
-[THEN]
-
[UNDEFINED] = [IF]
\ https://forth-standard.org/standard/core/Equal
\ = x1 x2 -- flag test x1=x2
ENDCODE
[THEN]
+[UNDEFINED] R> [IF]
+\ https://forth-standard.org/standard/core/Rfrom
+\ R> -- x R: x -- pop from return stack ; CALL #RFROM performs DOVAR
+CODE R>
+SUB #2,PSP \ 1
+MOV TOS,0(PSP) \ 3
+MOV @RSP+,TOS \ 2
+MOV @IP+,PC \ 4
+ENDCODE
+[THEN]
+
[UNDEFINED] HOLDS [IF]
\ https://forth-standard.org/standard/core/HOLDS
\ Adds the string represented by addr u to the pictured numeric output string
ENDCODE
[THEN]
-ASM XSCALE \ X = X*Cordic_Gain
-SUB #4,PSP
-MOV TOS,2(PSP)
-MOV X,0(PSP) \ 1st operand = X
-MOV T_SCALE(W),TOS \ 3 2nd operand = CORDIC Gain * 65536 ; IS UNSIGNED!
-\ https://forth-standard.org/standard/core/MTimes
-\ M* n1 n2 -- dlo dhi signed 16*16->32 multiply
-\ -----------------------------------------------
-\ CODE M*
-\ -----------------------------------------------
-MOV @PSP,S \ S= n1
-CMP #0,S \ n1 > -1 ?
-S< IF
- XOR #-1,0(PSP) \ n1 --> u1
- ADD #1,0(PSP) \
-THEN
-\ XOR TOS,S \ S contains sign of result
-\ CMP #0,TOS \ n2 > -1 ?
-\ S< IF
-\ XOR #-1,TOS \ n2 --> u2
-\ ADD #1,TOS \
-\ THEN
-\ PUSHM #2,IP \ UMSTAR use S,T,W,X,Y, we save IP,S
-PUSHM #6,IP \ UMSTAR use S,T,W,X,Y, we save IP,S,T,W,X,Y
-LO2HI \ -- ud1 u2
-UM*
-HI2LO
-\ POPM #2,IP \ pop S,IP
-POPM #6,IP \ pop Y,X,W,T,S,IP
-CMP #0,S \ sign of result > -1 ?
-S< IF
- XOR #-1,0(PSP) \ ud --> d
- XOR #-1,TOS
- ADD #1,0(PSP)
- ADDC #0,TOS
-THEN
-\ MOV @IP+,PC
-\ ENDCODE
-\ -----------------------------------------------
-MOV TOS,X \ hi result --> X
-ADD #2,PSP
-MOV @PSP+,TOS \ restore data stack
-MOV @RSP+,PC \ RET
+ASM XSCALE \ X --> X*Cordic_Gain
+\ T.I. UNSIGNED MULTIPLY SUBROUTINE: U1 x U2 -> Ud
+\ https://forth-standard.org/standard/core/UMTimes
+\ UM* u1 u2 -- ud unsigned 16x16->32 mult.
+ MOV T_SCALE(W),rDOCON \ rDOCON=MR, X=MDlo
+UMSTAR1 MOV #0,Y \ 1 MDhi=0
+ MOV #0,S \ 1 RES0=0
+ MOV #0,T \ 1 RES1=0
+ MOV #1,W \ 1 BIT TEST REGISTER
+BEGIN BIT W,rDOCON \ 1 TEST ACTUAL BIT MRlo
+ 0<> IF ADD X,S \ 1 IF 1: ADD MDlo TO RES0
+ ADDC Y,T \ 1 ADDC MDhi TO RES1
+ THEN ADD X,X \ 1 (RLA LSBs) MDlo x 2
+ ADDC Y,Y \ 1 (RLC MSBs) MDhi x 2
+ ADD W,W \ 1 (RLA) NEXT BIT TO TEST
+U>= UNTIL \ S = RESlo, T=REShi
+ MOV T,X \ 2 IF BIT IN CARRY: FINISHED 10~ loop
+ MOV #XDOCON,rDOCON \ restore rDOCON
+ MOV @RSP+,PC \ RET
ENDASM
-
-
[ELSE] ; hardware multiplier
[UNDEFINED] F#S [IF]
ENDCODE
[THEN]
-ASM XSCALE \ X = X*Cordic_Gain
-MOV X,&MPY \ 3 Load 1st operand
-MOV T_SCALE(W),&OP2 \ 3 CORDIC Gain * 65536
-MOV &RES1,X \ 3 hi result = hypothenuse
-MOV @RSP+,PC \ RET
+ASM XSCALE \ X = X*Cordic_Gain
+MOV T_SCALE(W),&MPYS32L \ 3 CORDIC Gain * 65536
+MOV #0,&MPYS32H
+MOV X,&OP2 \ 3 Load 1st operand
+MOV &RES1,X \ 3 hi result
+MOV @RSP+,PC \ RET
ENDASM
-[THEN] ; endcase of hardware multiplier
-
+[THEN] ; end of hardware multiplier
-[UNDEFINED] F. [IF]
-CODE F. \ display a Q15.16 number with 4/5/16 digits after comma
-MOV TOS,S \ S = sign
-MOV #4,T \ T = 4 preset 4 digits for base 16 and by default
-MOV &BASEADR,W
-CMP ##10,W
-0= IF \ if base 10
- ADD #1,T \ T = 5 set 5 digits
-ELSE
- CMP #%10,W
- 0= IF \ if base 2
- MOV #16,T \ T = 16 set 16 digits
- THEN
-THEN
-PUSHM #3,IP \ R-- IP sign #digit
-LO2HI
- <# DABS \ -- uQlo uQhi R-- IP sign #digit
- R> F#S \ -- uQhi 0 R-- IP sign
- $2C HOLD \ $2C = char ','
- #S \ -- 0 0
- R> SIGN #> \ -- addr len R-- IP
- TYPE $20 EMIT \ --
-;
-
-[THEN]
-
-CODE POL2REC \ u f -- X Y
-\ input ; u = module {1000...16384}, f = angle (15Q16 number) in degrees {1,0...89,0}
+CODE POL2REC \ u F -- X Y
+\ input ; u = module {1000...16384}, F = angle (15Q16 number) in degrees {-89,9...89,9}
\ output ; X Y
-\ TOS = fhi, 0(PSP) = flo, 2(PSP) = u
+\ TOS = Fhi, 0(PSP) = Flo, 2(PSP) = u
PUSH IP \ save IP before use
-MOV @PSP+,Y \ Y = flo
-SWPB Y
-AND #$00FF,Y
-SWPB TOS
-AND #$FF00,TOS
-BIS Y,TOS \ -- module angle*256
+MOV @PSP+,&MPY32L \ multiply angle by 286
+MOV TOS,&MPY32H
+MOV #286,&OP2
+MOV &RES0,Y
+MOV &RES1,TOS \ -- module angle*286
\ =====================
\ CORDIC 16 bits engine
\ =====================
MOV #-1,IP \ IP = i-1
MOV @PSP,X \ X = Xi
MOV #0,Y \ Y = Yi
- BEGIN \ i loops with init i = 0
- ADD #1,IP
+BEGIN \ i loops with init i = -1
+ ADD #1,IP \ i = i+1
MOV X,S \ S = Xi to be right shifted
MOV Y,T \ T = Yi to be right shifted
MOV #0,W \
THEN
CMP #0,TOS \ if angle*256 = 0 quit loop
0<> WHILE \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
- CMP #14,IP
- 0= UNTIL
+ CMP #14,IP \ IP = size of ARC_TAN table ?
+0= UNTIL
THEN \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
\ multiply cos by factor scale
CALL #XSCALE
\ REC2POL version with inputs scaling, to increase the accuracy of the angle:
\ REC2POL X Y -- u f
-\ input : X < 16384, |Y| < 16384
+\ input : X < 16384, Y < 16384
\ output ; u = hypothenuse, f = angle (15Q16 number) in degrees
\ rounded hypothenuse, 1 mn accuracy angle
CODE REC2POL \ X Y -- u f
XOR #-1,T
ADD #1,T
THEN
-\ 2- abort if null inputs
-MOV #-1,TOS \ set TOS TRUE for the two ABORT" below
+\ 2- calculate S = |X|
MOV X,S
-ADD T,S
-0= IF
- LO2HI
- ABORT" null inputs"
- HI2LO
+CMP #0,S
+S< IF
+ XOR #-1,S
+ ADD #1,S
THEN
-\ 3- select max of X,|Y|
-CMP X,T
-U< IF \ X > |Y|
- MOV X,T
+\ 3- abort if null inputs
+MOV #-1,TOS \ set TOS TRUE for the two ABORT" below
+CMP #0,X
+0= IF
+ CMP #0,Y
+ 0= IF
+ LO2HI
+ ABORT" null inputs!"
+ HI2LO
+ THEN
THEN
-\ 4- abort if X or |Y| >= 16384
+\ 4- select max of |X|,|Y|
+CMP S,T
+U< IF \ |X| > |Y|
+ MOV S,T
+THEN
+\ 5- abort if |X| or |Y| >= 16384
CMP #16384,T
U>= IF
LO2HI
- ABORT" x or |y| >= 16384"
+ ABORT" |x| or |y| >= 16384"
HI2LO
THEN
-\ 5- multiply inputs by 2^n scale factor
+\ 6- multiply inputs by 2^n scale factor
MOV #1,S \ init scale factor
-RLAM #3,T \ test bit 2^13
+RLAM #3,T \ test bit 2^13 of max(X,Y)
GOTO FW1
BEGIN
ADD X,X \ X=X*2
ADD T,T \ to test next bit 2^(n-1)
FW1
U>= UNTIL \ until carry set
-\ 6- save IP and scale factor n
+\ 7- save IP and scale factor n
PUSHM #2,IP \ push IP,S
\ ==================
\ CORDIC engine
\ ==================
MOV #-1,IP \ IP = i-1, X = Xi, Y = Yi
MOV #0,TOS \ init z=0
- BEGIN \ i loops with init: i = 0
- ADD #1,IP
+ BEGIN \ i loops with init: i = -1
+ ADD #1,IP \ i = i+1
MOV X,S \ S = Xi to be right shifted
MOV Y,T \ T = Yi to be right shifted
MOV #0,W \ W = right shift loop count
0= UNTIL \ 6~ loop
ADD W,W \ W = 2i = T_SCALE displacement
CMP #0,Y \ Y sign ?
- 0>= IF \ Y >= 0 : Rotate counter-clockwise
+ S>= IF \ Y >= 0 : Rotate counter-clockwise
ADD T,X \ Xi+1 = Xi + ( Yi >> i)
SUB S,Y \ Yi+1 = Yi - ( Xi >> i)
ADD T_ARCTAN(W),TOS
FW1 RRA S \ shift right scale factor
U>= UNTIL \ until carry set
MOV X,0(PSP)
-\ multiply z by 256 to display it as a Q15.16 number
-MOV TOS,Y \ Y = future fractional part of f
-SWPB TOS
-AND #$00FF,TOS
-SXT TOS \ integer part of f
-SWPB Y
-AND #$FF00,Y
-SUB #2,PSP
-MOV Y,0(PSP) \ fractional part of f
+
+\ divide z by 286 to display it as a Q15.16 number
+SUB #4,PSP \ -- X * * Zhi
+MOV TOS,rDOCON \ -- rDOCON as sign of QUOT
+CMP #0,rDOCON
+S< IF
+ XOR #-1,TOS
+ ADD #1,TOS
+THEN
+MOV #0,2(PSP) \ -- X Zlo * Zhi
+MOV TOS,0(PSP) \ -- X Zlo Zhi Zhi
+MOV #286,TOS \ -- X Zlo Zhi DIV
+CALL #MUSMOD \ -- X rem QUOTlo QUOThi
+MOV @PSP+,0(PSP) \ remove remainder
+CMP #0,rDOCON
+S< IF
+ XOR #-1,0(PSP)
+ XOR #-1,TOS
+ ADD #1,0(PSP)
+ ADDC #0,TOS
+THEN
+MOV #XDOCON,rDOCON
MOV @IP+,PC
-ENDCODE \
+ENDCODE
+
+
+[UNDEFINED] F. [IF]
+CODE F. \ display a Q15.16 number with 4/5/16 digits after comma
+MOV TOS,S \ S = sign
+MOV #4,T \ T = 4 preset 4 digits for base 16 and by default
+MOV &BASEADR,W
+CMP ##10,W
+0= IF \ if base 10
+ ADD #1,T \ T = 5 set 5 digits
+ELSE
+ CMP #%10,W
+ 0= IF \ if base 2
+ MOV #16,T \ T = 16 set 16 digits
+ THEN
+THEN
+PUSHM #3,IP \ R-- IP sign #digit
+LO2HI
+ <# DABS \ -- uQlo uQhi R-- IP sign #digit
+ R> F#S \ -- uQhi 0 R-- IP sign
+ $2C HOLD \ $2C = char ','
+ #S \ -- 0 0
+ R> SIGN #> \ -- addr len R-- IP
+ TYPE $20 EMIT \ --
+;
+
+[THEN]
PWR_HERE
ENDCODE
[THEN]
+ECHO
+
[UNDEFINED] ROT [IF] \
\ https://forth-standard.org/standard/core/ROT
\ ROT x1 x2 x3 -- x2 x3 x1
ENDCODE
[THEN]
-[UNDEFINED] DO [IF] \ define DO LOOP +LOOP
-\ https://forth-standard.org/standard/core/DO
-\ DO -- DOadr L: -- 0
-CODE DO \ immediate
-SUB #2,PSP \
-MOV TOS,0(PSP) \
-ADD #2,&DP \ make room to compile xdo
-MOV &DP,TOS \ -- HERE+2
-MOV #XDO,-2(TOS) \ compile xdo
-ADD #2,&LEAVEPTR \ -- HERE+2 LEAVEPTR+2
-MOV &LEAVEPTR,W \
-MOV #0,0(W) \ -- HERE+2 L-- 0
-MOV @IP+,PC
-ENDCODE IMMEDIATE
-
-\ https://forth-standard.org/standard/core/LOOP
-\ LOOP DOadr -- L-- an an-1 .. a1 0
-CODE LOOP \ immediate
- MOV #XLOOP,X
-BW1 ADD #4,&DP \ make room to compile two words
- MOV &DP,W
- MOV X,-4(W) \ xloop --> HERE
- MOV TOS,-2(W) \ DOadr --> HERE+2
-BEGIN \ resolve all "leave" adr
- MOV &LEAVEPTR,TOS \ -- Adr of top LeaveStack cell
- SUB #2,&LEAVEPTR \ --
- MOV @TOS,TOS \ -- first LeaveStack value
- CMP #0,TOS \ -- = value left by DO ?
-0<> WHILE
- MOV W,0(TOS) \ move adr after loop as UNLOOP adr
-REPEAT
- MOV @PSP+,TOS
- MOV @IP+,PC
-ENDCODE IMMEDIATE
-
-\ https://forth-standard.org/standard/core/PlusLOOP
-\ +LOOP adrs -- L-- an an-1 .. a1 0
-CODE +LOOP \ immediate
-MOV #XPLOOP,X
-GOTO BW1
-ENDCODE IMMEDIATE
-[THEN]
-
-: 1000CORDIC
-500 0 DO
- POL2REC REC2POL \ 2 CORDIC op. * 500 loops = 1000 CORDIC
-LOOP
-;
-
-ECHO
-
; -----------------------------------------------------------
; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
; -----------------------------------------------------------
-\
+
10000 89,0 POL2REC . . ; sin, cos -->
10000 75,0 POL2REC . . ; sin, cos -->
10000 60,0 POL2REC . . ; sin, cos -->
16384 30,0 POL2REC SWAP . . ; x, y -->
16384 45,0 POL2REC SWAP . . ; x, y -->
16384 60,0 POL2REC SWAP . . ; x, y -->
+
+\
+10000 -89,0 POL2REC . . ; sin, cos -->
+10000 -75,0 POL2REC . . ; sin, cos -->
+10000 -60,0 POL2REC . . ; sin, cos -->
+10000 -45,0 POL2REC . . ; sin, cos -->
+10000 -30,0 POL2REC . . ; sin, cos -->
+10000 -15,0 POL2REC . . ; sin, cos -->
+10000 -1,0 POL2REC . . ; sin, cos -->
+\ module phase -- X Y
+16384 -30,0 POL2REC SWAP . . ; x, y -->
+16384 -45,0 POL2REC SWAP . . ; x, y -->
+16384 -60,0 POL2REC SWAP . . ; x, y -->
+
+\
+-10000 89,0 POL2REC . . ; sin, cos -->
+-10000 75,0 POL2REC . . ; sin, cos -->
+-10000 60,0 POL2REC . . ; sin, cos -->
+-10000 45,0 POL2REC . . ; sin, cos -->
+-10000 30,0 POL2REC . . ; sin, cos -->
+-10000 15,0 POL2REC . . ; sin, cos -->
+-10000 1,0 POL2REC . . ; sin, cos -->
+\ module phase -- X Y
+-16384 30,0 POL2REC SWAP . . ; x, y -->
+-16384 45,0 POL2REC SWAP . . ; x, y -->
+-16384 60,0 POL2REC SWAP . . ; x, y -->
+\
+
+-10000 -89,0 POL2REC . . ; sin, cos -->
+-10000 -75,0 POL2REC . . ; sin, cos -->
+-10000 -60,0 POL2REC . . ; sin, cos -->
+-10000 -45,0 POL2REC . . ; sin, cos -->
+-10000 -30,0 POL2REC . . ; sin, cos -->
+-10000 -15,0 POL2REC . . ; sin, cos -->
+-10000 -1,0 POL2REC . . ; sin, cos -->
+\ module phase -- X Y
+-16384 -30,0 POL2REC SWAP . . ; x, y -->
+-16384 -45,0 POL2REC SWAP . . ; x, y -->
+-16384 -60,0 POL2REC SWAP . . ; x, y -->
\
\ 16384 -8192 REC2POL F. . ; --> abort
\ 0 0 REC2POL F. . ; --> abort
+-2 1 REC2POL F. . ; phase module -->
+-2 -1 REC2POL F. . ; phase module -->
+-20 10 REC2POL F. . ; phase module -->
+-20 -10 REC2POL F. . ; phase module -->
+-200 100 REC2POL F. . ; phase module -->
+-100 -100 REC2POL F. . ; phase module -->
+-2000 1000 REC2POL F. . ; phase module -->
+-1000 -1000 REC2POL F. . ; phase module -->
+-16000 8000 REC2POL F. . ; phase module -->
+-16000 -8000 REC2POL F. . ; phase module -->
+16000 0 REC2POL F. . ; phase module -->
+0 16000 REC2POL F. . ; phase module -->
+\ 16384 -8192 REC2POL F. . ; --> abort
+\ 0 0 REC2POL F. . ; --> abort
10000 89,0 POL2REC REC2POL ROT . F.
10000 75,0 POL2REC REC2POL ROT . F.
10000 7,125 POL2REC REC2POL ROT . F.
10000 1,0 POL2REC REC2POL ROT . F.
-10000 89,0 1000CORDIC ROT . F.
-10000 75,0 1000CORDIC ROT . F.
-10000 60,0 1000CORDIC ROT . F.
-10000 45,0 1000CORDIC ROT . F.
-10000 30,0 1000CORDIC ROT . F.
-10000 26,565 1000CORDIC ROT . F.
-10000 15,0 1000CORDIC ROT . F.
-10000 14,036 1000CORDIC ROT . F.
-10000 7,125 1000CORDIC ROT . F.
-10000 1,0 1000CORDIC ROT . F.
-
-PWR_STATE \ remove tests
+
OSDN Git Service
