-; CORDIC.f
+\ -*- coding: utf-8 -*-
\ see CORDICforDummies.pdf
\
+\ to see kernel options, download FastForthSpecs.f
+\ FastForth kernel options: MSP430ASSEMBLER, CONDCOMP, FIXPOINT_INPUT
\
-\ FAST-FORTH V208.
-\ compile FAST-FORTH with at least these options: ASSEMBLER, CONDCOMP, FIXPOINT_INPUT, LOWERCASE.
-
-\ TARGET Current Selection (used by preprocessor GEMA to load the pattern: \config\gema\TARGET.pat)
+\
+\ 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:
+\
+\ OR
+\
+\ drag and drop this file onto SendSourceFileToTarget.bat
+\ then select your TARGET when asked.
\
+\
+\ REGISTERS USAGE
+\ rDODOES to rEXIT must be saved before use and restored after
+\ scratch registers Y to S are free for use
+\ under interrupt, IP is free for use
+\
+\ PUSHM order : PSP,TOS, IP, S, T, W, X, Y, rEXIT, rDOVAR, rDOCON, rDODOES
+\ example : PUSHM #6,IP pushes IP,S,T,W,X,Y registers to return stack
+\
+\ POPM order : rDODOES, rDOCON, rDOVAR, rEXIT, Y, X, W, T, S, IP,TOS,PSP
+\ example : POPM #6,IP pulls Y,X,W,T,S,IP registers from return stack
+\
+\ FORTH conditionnals: unary{ 0= 0< 0> }, binary{ = < > U< }
+\
+\ 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<
+
+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 #308,TOS \ FastForth V3.8
+COLON
+$0D EMIT \ return to column 1 without CR
+ABORT" FastForth V3.8 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]
+
+MARKER {CORDIC}
+
+
\ CORDIC USES
\ OPERATION | MODE | INITIALIZE x y z | DIRECTION | RESULT | post operation
\ --------------|-----------|-----------------------|---------------|-------------------|
\ Gi = CORDIC gain for i iterations; Gi < 1
\
-[DEFINED] {CORDIC} [IF] {CORDIC} [THEN] \ remove {CORDIC}
+CREATE T_ARCTAN \ ArcTan table
+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
-MARKER {CORDIC}
+CREATE T_SCALE \ 1/Gi table
+46340 , \ = 65536 * cos(45)
+41448 , \ = 65536 * cos(45) * cos(26.565)
+40211 , \ = 65536 * cos(45) * cos(26.565) * cos(14.036)
+39900 , \ = 65536 * cos(45) * cos(26.565) * cos(14.036) * ...
+39822 ,
+39803 ,
+39798 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+
+[UNDEFINED] = [IF]
+\ https://forth-standard.org/standard/core/Equal
+\ = x1 x2 -- flag test x1=x2
+CODE =
+SUB @PSP+,TOS \ 2
+0<> IF \ 2
+ AND #0,TOS \ 1
+ MOV @IP+,PC \ 4
+THEN
+XOR #-1,TOS \ 1 flag Z = 1
+MOV @IP+,PC \ 4
+ENDCODE
+[THEN]
+
+\ https://forth-standard.org/standard/core/Uless
+\ U< u1 u2 -- flag test u1<u2, unsigned
+[UNDEFINED] U< [IF]
+CODE U<
+SUB @PSP+,TOS \ 2 u2-u1
+0<> IF
+ MOV #-1,TOS \ 1
+ U< IF \ 2 flag
+ AND #0,TOS \ 1 flag Z = 1
+ THEN
+THEN
+MOV @IP+,PC \ 4
+ENDCODE
+[THEN]
-[UNDEFINED] {FIXPOINT} [IF] \ define words to display angle as Q15.16 number.
+[UNDEFINED] DABS [IF]
+\ https://forth-standard.org/standard/double/DABS
+\ DABS d1 -- |d1| absolute value
+CODE DABS
+AND #-1,TOS \ clear V, set N
+S< IF \
+ XOR #-1,0(PSP) \ 4
+ XOR #-1,TOS \ 1
+ ADD #1,0(PSP) \ 4
+ ADDC #0,TOS \ 1
+THEN
+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] HOLDS [IF]
\ https://forth-standard.org/standard/core/HOLDS
\ Adds the string represented by addr u to the pictured numeric output string
\ compilation use: <# S" string" HOLDS #>
-\ free chars area in the 32+2 bytes HOLD buffer sized for a 32 bits {hexa,decimal,binary} number = {26,23,2}.
+\ free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
\ (2 supplementary bytes are room for sign - and decimal point)
\ C HOLDS addr u --
CODE HOLDS
- MOV @PSP+,X \ 2
- ADD TOS,X \ 1 src
- MOV &HP,Y \ 3 dst
-BEGIN SUB #1,X \ 1 src-1
- SUB #1,TOS \ 1 cnt-1
-U>= WHILE SUB #1,Y \ 1 dst-1
- MOV.B @X,0(Y) \ 4
-REPEAT MOV Y,&HP \ 3
- MOV @PSP+,TOS \ 2
- MOV @IP+,PC \ 4 15 words
+ MOV @PSP+,X \ 2 X=src
+BW3 ADD TOS,X \ 1 X=src_end
+ MOV &HP,Y \ 3 Y=dst
+BEGIN SUB #1,X \ 1 src-1
+ SUB #1,TOS \ 1 cnt-1
+U>= WHILE SUB #1,Y \ 1 dst-1
+ MOV.B @X,0(Y) \ 4
+REPEAT MOV Y,&HP \ 3
+ MOV @PSP+,TOS \ 2
+ MOV @IP+,PC \ 4 15 words
ENDCODE
+[THEN]
+
+$81EF DEVICEID @ U<
+DEVICEID @ $81F3 U<
+= [IF] ; MSP430FR413x subfamily without hardware_MPY
-\ F#S Qlo Qhi u -- Qhi 0 convert fractionnal part of Q15.16 fixed point number with u digits
+[UNDEFINED] F#S [IF]
+\ F#S Qlo Qhi len -- Qhi 0 convert fractional part Qlo of Q15.16 fixed point number
+\ with len digits
CODE F#S
- MOV 2(PSP),X \ -- Qlo Qhi u X = Qlo
- MOV @PSP,2(PSP) \ -- Qhi Qhi u
- MOV X,0(PSP) \ -- Qhi Qlo u
- MOV TOS,T \ T = limit
- MOV #0,S \ S = count
-BEGIN MOV @PSP,&MPY \ Load 1st operand
- MOV &BASE,&OP2 \ Load 2nd operand
- MOV &RES0,0(PSP) \ -- Qhi RESlo x low result on stack
- MOV &RES1,TOS \ -- Qhi RESlo REShi high result in TOS
- CMP #10,TOS \ digit to char
+ MOV @PSP,S \ -- Qlo Qhi len S = Qhi
+ MOV #0,T \ T = count
+ PUSHM #3,IP \ R-- IP Qhi count
+ MOV 2(PSP),0(PSP) \ -- Qlo Qlo len
+ MOV TOS,2(PSP) \ -- len Qlo len
+BEGIN MOV &BASEADR,TOS \ -- len Qlo base
+ LO2HI
+ UM* \ u1 u2 -- RESlo REShi
+ HI2LO \ -- len RESlo digit
+ CMP #10,TOS \ digit to char
U>= IF ADD #7,TOS
- THEN ADD #$30,TOS
- MOV.B TOS,HOLDS_ORG(S) \ -- Qhi RESlo char char to string
- ADD #1,S \ count+1
- CMP T,S \ count=limit ?
-0= UNTIL MOV #0,0(PSP) \ -- Qhi 0 REShi
- MOV T,TOS \ -- Qhi 0 limit
- SUB #2,PSP \ -- Qhi 0 x len
- MOV #HOLDS_ORG,0(PSP) \ -- Qhi 0 addr len
- JMP HOLDS
+ THEN ADD #$30,TOS \ -- len RESlo char
+ MOV @RSP,T \ T=count
+ MOV.B TOS,HOLDS_ORG(T) \ char to string_org(T)
+ ADD #1,T \ count+1
+ MOV T,0(RSP) \
+ CMP 2(PSP),T \ -- len RESlo char count=len ?
+U>= UNTIL POPM #3,IP \ S=Qhi, T=len
+ MOV T,TOS \ -- len RESlo len
+ MOV S,2(PSP) \ -- Qhi RESlo len
+ MOV #0,0(PSP) \ -- Qhi 0 len
+ MOV #HOLDS_ORG,X \ -- Qhi 0 len X=HOLDS_ORG
+ GOTO BW3 \ 36~ JMP HOLDS
ENDCODE
+[THEN]
-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 &BASE,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 SPACE \ --
-;
+HDNCODE 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
+ENDCODE
-[THEN] \ end of [UNDEFINED] {FIXPOINT}
+[ELSE] ; hardware multiplier
-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
-28 , \ 256 * 0.112
-14 , \ 256 * 0.056
-7 , \ 256 * 0.028
-4 , \ 256 * 0.014
-2 , \ 256 * 0.007
-1 , \ 256 * 0.003
+[UNDEFINED] F#S [IF]
+\ F#S Qlo Qhi u -- Qhi 0 convert fractionnal part of Q15.16 fixed point number
+\ with u digits
+CODE F#S
+ MOV 2(PSP),X \ -- Qlo Qhi u X = Qlo
+ MOV @PSP,2(PSP) \ -- Qhi Qhi u
+ MOV X,0(PSP) \ -- Qhi Qlo u
+ MOV TOS,T \ T = len
+ MOV #0,S \ S = count
+BEGIN MOV @PSP,&MPY \ Load 1st operand
+ MOV &BASEADR,&OP2 \ Load 2nd operand
+ MOV &RES0,0(PSP) \ -- Qhi RESlo x low result on stack
+ MOV &RES1,TOS \ -- Qhi RESlo REShi high result in TOS
+ CMP #10,TOS \ digit to char
+ U>= IF ADD #7,TOS
+ THEN ADD #$30,TOS
+ MOV.B TOS,HOLDS_ORG(S) \ -- Qhi RESlo char char to string
+ ADD #1,S \ count+1
+ CMP T,S \ count=len ?
+0= UNTIL MOV T,TOS \ -- len RESlo len
+ MOV #0,0(PSP) \ -- Qhi 0 len
+ MOV #HOLDS_ORG,X \ -- Qhi 0 len X=HOLDS_ORG
+ GOTO BW3 \ 35~ JMP HOLDS+2
+ENDCODE
+[THEN]
-CREATE T_SCALE \ 1/Gi table
-46340 , \ = 65536 * cos(45)
-41448 , \ = 65536 * cos(45) * cos(26.565)
-40211 , \ = 65536 * cos(45) * cos(26.565) * cos(14.036)
-39900 , \ = 65536 * cos(45) * cos(26.565) * cos(14.036) ....
-39822 ,
-39803 ,
-39798 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
+HDNCODE 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
+ENDCODE
+[THEN] ; end of hardware multiplier
-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
-MOV @PSP+,Y \ Y = flo
-SWPB Y
-AND #$00FF,Y
-SWPB TOS
-AND #$FF00,TOS
-BIS Y,TOS \ -- module angle*256
+\ TOS = Fhi, 0(PSP) = Flo, 2(PSP) = u
PUSH IP \ save IP before use
-\ ==================
-\ CORDIC engine
-\ ==================
+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 \
RRA S \ (Xi >> 1)
RRA T \ (Yi >> 1)
ADD #1,W
- FW1 CMP IP,W \ W = i ?
+FW1 CMP IP,W \ W = i ?
0= UNTIL \ loop back if W < i
ADD W,W \ W = 2i = T_SCALE displacement
CMP #0,TOS \ TOS = z
SUB S,Y \ Yi+1 = Yi - ( Xi >> i)
ADD T_ARCTAN(W),TOS
THEN
- CMP #0,TOS
+ 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
-MOV X,&MPY \ 3 Load 1st operand
-MOV T_SCALE(W),&OP2 \ 3 Load 2nd operand
-MOV &RES1,0(PSP) \ 3 hi result = cos
+CALL #XSCALE
+MOV X,0(PSP) \ 3 hi result = cos
\ multiply sin by factor scale
-MOV Y,&MPY \ 3 Load 1st operand
-MOV T_SCALE(W),&OP2 \ 3 Load 2nd operand
-MOV &RES1,TOS \ 3 hi result = sin
+MOV Y,X \ 3
+CALL #XSCALE
+MOV X,TOS \ 3 hi result = sin
\ ==================
\ endof CORDIC engine \ X = cos, Y = sin
\ ==================
MOV @IP+,PC
ENDCODE \ -- cos sin
-\
-10000 89,0 POL2REC . . ; sin, cos -->
-10000 60,0 POL2REC . . ; sin, cos -->
-10000 45,0 POL2REC . . ; sin, cos -->
-10000 30,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 -->
-\
-
-
\ 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- 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
-\ 3- select max of X,|Y|
-CMP X,T
-U< IF \ X > |Y|
- MOV X,T
+\ 4- select max of |X|,|Y|
+CMP S,T
+U< IF \ |X| > |Y|
+ MOV S,T
THEN
-\ 4- abort if X or |Y| >= 16384
+\ 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
RRA S \ (X >> i)
RRA T \ (Y >> i)
ADD #1,W \
- FW1 CMP IP,W \ W = i ?
+FW1 CMP IP,W \ W = i ?
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
ADD S,Y \ Yi+1 = Yi + ( Xi >> i)
SUB T_ARCTAN(W),TOS
THEN
- CMP #0,Y
- 0<> WHILE \ else goto THEN below
- CMP #14,IP
- 0= UNTIL
- THEN
+ CMP #0,Y \
+ 0<> WHILE \ if Y = 0 quit loop ---+
+ CMP #14,IP \ |
+ 0= UNTIL \ |
+ THEN \ <---------------------+
\ multiply x by 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
+CALL #XSCALE \ 3 hi result = hypothenuse
\ ==================
\ endof CORDIC engine \ X = hypothenuse, TOS = 256*angle
\ ==================
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
+
+[UNDEFINED] SWAP [IF]
+\ https://forth-standard.org/standard/core/SWAP
+\ SWAP x1 x2 -- x2 x1 swap top two items
+CODE SWAP
+MOV @PSP,W \ 2
+MOV TOS,0(PSP) \ 3
+MOV W,TOS \ 1
+MOV @IP+,PC \ 4
+ENDCODE
+[THEN]
+
+ECHO
+
+[UNDEFINED] ROT [IF] \
+\ https://forth-standard.org/standard/core/ROT
+\ ROT x1 x2 x3 -- x2 x3 x1
+CODE ROT
+MOV @PSP,W \ 2 fetch x2
+MOV TOS,0(PSP) \ 3 store x3
+MOV 2(PSP),TOS \ 3 fetch x1
+MOV W,2(PSP) \ 3 store x2
+MOV @IP+,PC
+ENDCODE
+[THEN]
+
+; -----------------------------------------------------------
+; 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 -->
+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 -->
+\
+
+-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 -->
+\
-RST_HERE
2 1 REC2POL F. . ; phase module -->
2 -1 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
+16000 0 REC2POL F. . ; phase module -->
+0 16000 REC2POL F. . ; phase module -->
+\ 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 45,0 POL2REC REC2POL ROT . F.
10000 30,0 POL2REC REC2POL ROT . F.
10000 26,565 POL2REC REC2POL ROT . F.
+10000 15,0 POL2REC REC2POL ROT . F.
10000 14,036 POL2REC REC2POL ROT . F.
10000 7,125 POL2REC REC2POL ROT . F.
10000 1,0 POL2REC REC2POL ROT . F.
-: 2000CORDIC
-1000 0 DO
- POL2REC REC2POL \ 1000 loops
-LOOP
-;
-10000 89,0 2000CORDIC ROT . F.
-10000 75,0 2000CORDIC ROT . F.
-10000 60,0 2000CORDIC ROT . F.
-10000 45,0 2000CORDIC ROT . F.
-10000 30,0 2000CORDIC ROT . F.
-10000 26,565 2000CORDIC ROT . F.
-10000 14,036 2000CORDIC ROT . F.
-10000 7,125 2000CORDIC ROT . F.
-10000 1,0 2000CORDIC ROT . F.
OSDN Git Service
