MSP430-FORTH/CORDIC.f

   1 \ -*- coding: utf-8 -*-
   2
   3 ; ----------
   4 ; CORDIC.f
   5 ; ----------
   6 \ see CORDICforDummies.pdf
   7 \
   8 ; -----------------------------------------------------------
   9 ; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
  10 ; -----------------------------------------------------------
  11 \
  12 \ to see kernel options, download FastForthSpecs.f
  13 \ FastForth kernel options: MSP430ASSEMBLER, CONDCOMP, FIXPOINT_INPUT
  14 \
  15 \
  16 \ LP_MSP430FR2476
  17 \ MSP_EXP430FR5739  MSP_EXP430FR5969    MSP_EXP430FR5994    MSP_EXP430FR6989
  18 \ MSP_EXP430FR2433    MSP_EXP430FR2355    CHIPSTICK_FR2433
  19 \
  20 \ REGISTERS USAGE
  21 \ rDODOES to rEXIT must be saved before use and restored after
  22 \ scratch registers Y to S are free for use
  23 \ under interrupt, IP is free for use
  24 \
  25 \ PUSHM order : PSP,TOS, IP,  S,  T,  W,  X,  Y, rEXIT, rDOVAR, rDOCON, rDODOES
  26 \ example : PUSHM #6,IP pushes IP,S,T,W,X,Y registers to return stack
  27 \
  28 \ POPM  order :  rDODOES, rDOCON, rDOVAR, rEXIT,  Y,  X,  W,  T,  S, IP,TOS,PSP
  29 \ example : POPM #6,IP   pulls Y,X,W,T,S,IP registers from return stack
  30 \
  31 \ FORTH conditionnals:  unary{ 0= 0< 0> }, binary{ = < > U< }
  32 \
  33 \ ASSEMBLER conditionnal usage with IF UNTIL WHILE  S<  S>=  U<   U>=  0=  0<>  0>=
  34 \ ASSEMBLER conditionnal usage with ?JMP ?GOTO      S<  S>=  U<   U>=  0=  0<>  0<
  35
  36
  37 PWR_STATE
  38
  39 [UNDEFINED] {CORDIC} [IF]
  40
  41 [UNDEFINED] MARKER [IF]
  42 \  https://forth-standard.org/standard/core/MARKER
  43 \  MARKER
  44 \ ( "<spaces>name" -- )
  45 \ Skip leading space delimiters. Parse name delimited by a space. Create a definition for name
  46 \ with the execution semantics defined below.
  47 \
  48 \ name Execution: ( -- )
  49 \ Restore all dictionary allocation and search order pointers to the state they had just prior to the
  50 \ definition of name. Remove the definition of name and all subsequent definitions. Restoration
  51 \ of any structures still existing that could refer to deleted definitions or deallocated data space is
  52 \ not necessarily provided. No other contextual information such as numeric base is affected
  53 \
  54 : MARKER
  55 CREATE
  56 HI2LO
  57 MOV &LASTVOC,0(W)   \ [BODY] = LASTVOC
  58 SUB #2,Y            \ 1 Y = LFA
  59 MOV Y,2(W)          \ 3 [BODY+2] = LFA = DP to be restored
  60 ADD #4,&DP          \ 3 add 2 cells
  61 LO2HI
  62 DOES>
  63 HI2LO
  64 MOV @RSP+,IP        \ -- PFA
  65 MOV @TOS+,&INIVOC   \       set VOC_LINK value for RST_STATE
  66 MOV @TOS,&INIDP     \       set DP value for RST_STATE
  67 MOV @PSP+,TOS       \ --
  68 MOV #RST_STATE,PC   \       execute RST_STATE, PWR_STATE then STATE_DOES
  69 ENDCODE
  70 [THEN]
  71
  72 MARKER {CORDIC}
  73
  74 [UNDEFINED] SWAP [IF]
  75 \ https://forth-standard.org/standard/core/SWAP
  76 \ SWAP     x1 x2 -- x2 x1    swap top two items
  77 CODE SWAP
  78 MOV @PSP,W      \ 2
  79 MOV TOS,0(PSP)  \ 3
  80 MOV W,TOS       \ 1
  81 MOV @IP+,PC     \ 4
  82 ENDCODE
  83 [THEN]
  84
  85 [UNDEFINED] IF [IF]
  86 \ https://forth-standard.org/standard/core/IF
  87 \ IF       -- IFadr    initialize conditional forward branch
  88 CODE IF       \ immediate
  89 SUB #2,PSP              \
  90 MOV TOS,0(PSP)          \
  91 MOV &DP,TOS             \ -- HERE
  92 ADD #4,&DP            \           compile one word, reserve one word
  93 MOV #QFBRAN,0(TOS)      \ -- HERE   compile QFBRAN
  94 ADD #2,TOS              \ -- HERE+2=IFadr
  95 MOV @IP+,PC
  96 ENDCODE IMMEDIATE
  97 [THEN]
  98
  99 [UNDEFINED] THEN [IF]
 100 \ https://forth-standard.org/standard/core/THEN
 101 \ THEN     IFadr --                resolve forward branch
 102 CODE THEN               \ immediate
 103 MOV &DP,0(TOS)          \ -- IFadr
 104 MOV @PSP+,TOS           \ --
 105 MOV @IP+,PC
 106 ENDCODE IMMEDIATE
 107 [THEN]
 108
 109 [UNDEFINED] ELSE [IF]
 110 \ https://forth-standard.org/standard/core/ELSE
 111 \ ELSE     IFadr -- ELSEadr        resolve forward IF branch, leave ELSEadr on stack
 112 CODE ELSE     \ immediate
 113 ADD #4,&DP              \ make room to compile two words
 114 MOV &DP,W               \ W=HERE+4
 115 MOV #BRAN,-4(W)
 116 MOV W,0(TOS)            \ HERE+4 ==> [IFadr]
 117 SUB #2,W                \ HERE+2
 118 MOV W,TOS               \ -- ELSEadr
 119 MOV @IP+,PC
 120 ENDCODE IMMEDIATE
 121 [THEN]
 122
 123 [UNDEFINED] BEGIN [IF]
 124 \ https://forth-standard.org/standard/core/BEGIN
 125 \ BEGIN    -- BEGINadr             initialize backward branch
 126 CODE BEGIN              \ immediate
 127 MOV #HERE,PC            \ BR HERE
 128 ENDCODE IMMEDIATE
 129 [THEN]
 130
 131 [UNDEFINED] UNTIL [IF]
 132 \ https://forth-standard.org/standard/core/UNTIL
 133 \ UNTIL    BEGINadr --             resolve conditional backward branch
 134 CODE UNTIL              \ immediate
 135     MOV #QFBRAN,X
 136 BW1 ADD #4,&DP          \ compile two words
 137     MOV &DP,W           \ W = HERE
 138     MOV X,-4(W)         \ compile Bran or QFBRAN at HERE
 139     MOV TOS,-2(W)       \ compile bakcward adr at HERE+2
 140     MOV @PSP+,TOS
 141     MOV @IP+,PC
 142 ENDCODE IMMEDIATE
 143 [THEN]
 144
 145 [UNDEFINED] AGAIN [IF]
 146 \ https://forth-standard.org/standard/core/AGAIN
 147 \ AGAIN    BEGINadr --             resolve uncondionnal backward branch
 148 CODE AGAIN     \ immediate
 149 MOV #BRAN,X
 150 GOTO BW1
 151 ENDCODE IMMEDIATE
 152 [THEN]
 153
 154 [UNDEFINED] WHILE [IF]
 155 \ https://forth-standard.org/standard/core/WHILE
 156 \ WHILE    BEGINadr -- WHILEadr BEGINadr
 157 : WHILE     \ immediate
 158 POSTPONE IF SWAP
 159 ; IMMEDIATE
 160 [THEN]
 161
 162 [UNDEFINED] REPEAT [IF]
 163 \ https://forth-standard.org/standard/core/REPEAT
 164 \ REPEAT   WHILEadr BEGINadr --     resolve WHILE loop
 165 : REPEAT
 166 POSTPONE AGAIN POSTPONE THEN
 167 ; IMMEDIATE
 168 [THEN]
 169
 170 [UNDEFINED] DO [IF]
 171 \ https://forth-standard.org/standard/core/DO
 172 \ DO       -- DOadr   L: -- 0
 173 CODE DO                 \ immediate
 174 SUB #2,PSP              \
 175 MOV TOS,0(PSP)          \
 176 ADD #2,&DP              \   make room to compile xdo
 177 MOV &DP,TOS             \ -- HERE+2
 178 MOV #XDO,-2(TOS)        \   compile xdo
 179 ADD #2,&LEAVEPTR        \ -- HERE+2     LEAVEPTR+2
 180 MOV &LEAVEPTR,W         \
 181 MOV #0,0(W)             \ -- HERE+2     L-- 0
 182 MOV @IP+,PC
 183 ENDCODE IMMEDIATE
 184 [THEN]
 185
 186 [UNDEFINED] LOOP [IF]
 187 \ https://forth-standard.org/standard/core/LOOP
 188 \ LOOP    DOadr --         L-- an an-1 .. a1 0
 189 CODE LOOP               \ immediate
 190     MOV #XLOOP,X
 191     ADD #4,&DP          \ make room to compile two words
 192     MOV &DP,W
 193     MOV X,-4(W)         \ xloop --> HERE
 194     MOV TOS,-2(W)       \ DOadr --> HERE+2
 195 BEGIN                   \ resolve all "leave" adr
 196     MOV &LEAVEPTR,TOS   \ -- Adr of top LeaveStack cell
 197     SUB #2,&LEAVEPTR    \ --
 198     MOV @TOS,TOS        \ -- first LeaveStack value
 199     CMP #0,TOS          \ -- = value left by DO ?
 200 0<> WHILE
 201     MOV W,0(TOS)        \ move adr after loop as UNLOOP adr
 202 REPEAT
 203     MOV @PSP+,TOS
 204     MOV @IP+,PC
 205 ENDCODE IMMEDIATE
 206 [THEN]
 207
 208
 209 [UNDEFINED] {FIXPOINT} [IF] \ define words to display angle as Q15.16 number.
 210
 211 [UNDEFINED] DABS [IF]
 212 \ https://forth-standard.org/standard/double/DABS
 213 \ DABS     d1 -- |d1|     absolute value
 214 CODE DABS
 215 AND #-1,TOS         \ clear V, set N
 216 S< IF               \ if positive (N=0)
 217     XOR #-1,0(PSP)  \ 4
 218     XOR #-1,TOS     \ 1
 219     ADD #1,0(PSP)   \ 4
 220     ADDC #0,TOS     \ 1
 221 THEN
 222 MOV @IP+,PC
 223 ENDCODE
 224 [THEN]
 225
 226 \ https://forth-standard.org/standard/core/HOLDS
 227 \ Adds the string represented by addr u to the pictured numeric output string
 228 \ compilation use: <# S" string" HOLDS #>
 229 \ free chars area in the 32+2 bytes HOLD buffer sized for a 32 bits {hexa,decimal,binary} number = {26,23,2}.
 230 \ (2 supplementary bytes are room for sign - and decimal point)
 231 \ C HOLDS    addr u --
 232 CODE HOLDS
 233 BW1         MOV @PSP+,X     \ 2
 234             ADD TOS,X       \ 1 src
 235             MOV &HP,Y       \ 3 dst
 236 BEGIN       SUB #1,X        \ 1 src-1
 237             SUB #1,TOS      \ 1 cnt-1
 238 U>= WHILE   SUB #1,Y        \ 1 dst-1
 239             MOV.B @X,0(Y)   \ 4
 240 REPEAT      MOV Y,&HP       \ 3
 241             MOV @PSP+,TOS   \ 2
 242             MOV @IP+,PC     \ 4  15 words
 243 ENDCODE
 244
 245 \ F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number with u digits
 246 CODE F#S
 247             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
 248             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
 249             MOV X,0(PSP)            \ -- Qhi Qlo u
 250             MOV TOS,T               \                   T = limit
 251             MOV #0,S                \                   S = count
 252 BEGIN       MOV @PSP,&MPY           \                   Load 1st operand
 253             MOV &BASEADR,&OP2          \                   Load 2nd operand
 254             MOV &RES0,0(PSP)        \ -- Qhi RESlo x        low result on stack
 255             MOV &RES1,TOS           \ -- Qhi RESlo REShi    high result in TOS
 256             CMP #10,TOS             \                   digit to char
 257     U>= IF  ADD #7,TOS
 258     THEN    ADD #$30,TOS
 259             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
 260             ADD #1,S                \                   count+1
 261             CMP T,S                 \                   count=limit ?
 262 0= UNTIL    MOV #0,0(PSP)           \ -- Qhi 0 REShi
 263             MOV T,TOS               \ -- Qhi 0 limit
 264             SUB #2,PSP              \ -- Qhi 0 x len
 265             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
 266             GOTO BW1                \ JMP HOLDS
 267 ENDCODE
 268
 269 [UNDEFINED] R> [IF]
 270 \ https://forth-standard.org/standard/core/Rfrom
 271 \ R>    -- x    R: x --   pop from return stack ; CALL #RFROM performs DOVAR
 272 CODE R>
 273 MOV rDOVAR,PC
 274 ENDCODE
 275 [THEN]
 276
 277 CODE F.             \ display a Q15.16 number with 4/5/16 digits after comma
 278 MOV TOS,S           \ S = sign
 279 MOV #4,T            \ T = 4     preset 4 digits for base 16 and by default
 280 MOV &BASEADR,W
 281 CMP ##10,W
 282 0= IF               \           if base 10
 283     ADD #1,T        \ T = 5     set 5 digits
 284 ELSE
 285     CMP #%10,W
 286     0= IF           \           if base 2
 287         MOV #16,T   \ T = 16    set 16 digits
 288     THEN
 289 THEN
 290 PUSHM #3,IP         \                   R-- IP sign #digit
 291 LO2HI
 292     <# DABS         \ -- uQlo uQhi      R-- IP sign #digit
 293     R> F#S          \ -- uQhi 0         R-- IP sign
 294     $2C HOLD        \                   $2C = char ','
 295     #S              \ -- 0 0
 296     R> SIGN #>      \ -- addr len       R-- IP
 297     TYPE $20 EMIT   \ --
 298 ;
 299
 300 [THEN] \ end of [UNDEFINED] {FIXPOINT}
 301
 302 \ CORDIC USES
 303 \   OPERATION   |   MODE    |   INITIALIZE x y z    |   DIRECTION   |     RESULT        | post operation
 304 \ --------------|-----------|-----------------------|---------------|-------------------|
 305 \ sine, cosine  | Rotation  | x=1, y=0,  z=angle    | Reduce z to 0 | cos=x*Gi,sin=y*Gi | mutiply by 1/Gi
 306 \ --------------|-----------|-----------------------|---------------|-------------------|
 307 \ Polar to Rect | Rotation  | x=magnit, y=0, Z=angle| Reduce z to 0 |  X=x*Gi, Y=y*Gi   | mutiply by 1/Gi
 308 \ --------------|-----------|-----------------------|---------------|-------------------|
 309 \ Rotation      | Rotation  | x=X, y=Y, z=angle     | Reduce z to 0 | X'=x*Gi,Y'=y*Gi   | <=== not implemented
 310 \ --------------|-----------|-----------------------|---------------|-------------------|
 311 \ Rect to Polar |  Vector   | x=X, y=Y, z=0         | Reduce y to 0 | hyp=x*Gi, angle=z | mutiply hyp by 1/Gi
 312 \ --------------|-----------|-----------------------|---------------|-------------------|
 313 \ Gi = CORDIC gain for i iterations; Gi < 1
 314 \
 315
 316 CREATE T_ARCTAN \ ArcTan table
 317 11520 ,         \ 256 * 45
 318 6801 ,          \ 256 * 26.565
 319 3593 ,          \ 256 * 14.036
 320 1824 ,          \ 256 * 7.125
 321 916 ,           \ 256 * 3.576
 322 458 ,           \ 256 * 1.790
 323 229 ,           \ 256 * 0.895
 324 115 ,           \ 256 * 0.448
 325 57 ,            \ 256 * 0.224
 326 29 ,            \ 256 * 0.112
 327 14 ,            \ 256 * 0.056
 328 7 ,             \ 256 * 0.028
 329 4 ,             \ 256 * 0.014
 330 2 ,             \ 256 * 0.007
 331 1 ,             \ 256 * 0.003
 332
 333 CREATE T_SCALE  \ 1/Gi table
 334 46340 ,         \ = 65536 * cos(45)
 335 41448 ,         \ = 65536 * cos(45) * cos(26.565)
 336 40211 ,         \ = 65536 * cos(45) * cos(26.565) * cos(14.036)
 337 39900 ,         \ = 65536 * cos(45) * cos(26.565) * cos(14.036) * ...
 338 39822 ,
 339 39803 ,
 340 39798 ,
 341 39797 ,
 342 39797 ,
 343 39797 ,
 344 39797 ,
 345 39797 ,
 346 39797 ,
 347 39797 ,
 348 39797 ,
 349
 350
 351 CODE POL2REC   \ u f -- X Y
 352 \ input ; u = module {1000...16384}, f = angle (15Q16 number) in degrees {1,0...89,0}
 353 \ output ; X Y
 354 \ TOS = fhi, 0(PSP) = flo, 2(PSP) = u
 355 PUSH IP             \ save IP before use
 356 MOV @PSP+,Y         \ Y = flo
 357 SWPB Y
 358 AND #$00FF,Y
 359 SWPB TOS
 360 AND #$FF00,TOS
 361 BIS Y,TOS           \ -- module angle*256
 362 \ =====================
 363 \ CORDIC 16 bits engine
 364 \ =====================
 365 MOV #-1,IP          \ IP = i-1
 366 MOV @PSP,X          \ X = Xi
 367 MOV #0,Y            \ Y = Yi
 368  BEGIN              \ i loops with init i = 0
 369     ADD #1,IP
 370     MOV X,S         \ S = Xi to be right shifted
 371     MOV Y,T         \ T = Yi to be right shifted
 372     MOV #0,W        \
 373     GOTO FW1
 374     BEGIN
 375         RRA S       \ (Xi >> 1)
 376         RRA T       \ (Yi >> 1)
 377         ADD #1,W
 378 FW1     CMP IP,W    \ W = i ?
 379     0= UNTIL        \ loop back if W < i
 380     ADD W,W         \ W = 2i = T_SCALE displacement
 381     CMP #0,TOS      \ TOS = z
 382     0>= IF          \ TOS >= 0 : Rotate clockwise
 383         SUB T,X     \ Xi+1 = Xi - ( Yi >> i)
 384         ADD S,Y     \ Yi+1 = Yi + ( Xi >> i)
 385         SUB T_ARCTAN(W),TOS
 386     ELSE            \ TOS < 0 : Rotate counter-clockwise
 387         ADD T,X     \ Xi+1 = Xi + ( Yi >> i)
 388         SUB S,Y     \ Yi+1 = Yi - ( Xi >> i)
 389         ADD T_ARCTAN(W),TOS
 390     THEN
 391     CMP #0,TOS      \ if angle*256 = 0 quit loop
 392     0<> WHILE       \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
 393         CMP #14,IP
 394  0= UNTIL
 395     THEN            \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
 396 \ multiply cos by factor scale
 397 MOV X,&MPY              \ 3     Load 1st operand
 398 MOV T_SCALE(W),&OP2     \ 3     Load 2nd operand
 399 MOV &RES1,0(PSP)        \ 3     hi result = cos
 400 \ multiply sin by factor scale
 401 MOV Y,&MPY              \ 3     Load 1st operand
 402 MOV T_SCALE(W),&OP2     \ 3     Load 2nd operand
 403 MOV &RES1,TOS           \ 3     hi result = sin
 404 \ ==================
 405 \ endof CORDIC engine   \ X = cos, Y = sin
 406 \ ==================
 407 MOV @RSP+,IP
 408 MOV @IP+,PC
 409 ENDCODE                 \ -- cos sin
 410
 411
 412 \ REC2POL version with inputs scaling, to increase the accuracy of the angle:
 413 \ REC2POL   X Y -- u f
 414 \ input : X < 16384, |Y| < 16384
 415 \ output ; u = hypothenuse, f = angle (15Q16 number) in degrees
 416 \ rounded hypothenuse, 1 mn accuracy angle
 417 CODE REC2POL        \ X Y -- u f
 418 MOV @PSP,X          \ X = Xi
 419 MOV TOS,Y           \ Y = Yi
 420 \ normalize X Y to 16384 maxi
 421 \ 1- calculate T = |Y|
 422 MOV Y,T
 423 CMP #0,T
 424 S< IF
 425     XOR #-1,T
 426     ADD #1,T
 427 THEN
 428 \ 2- abort if null inputs
 429 MOV #-1,TOS \ set TOS TRUE for the two ABORT" below
 430 MOV X,S
 431 ADD T,S
 432 0= IF
 433     LO2HI
 434         ABORT" null inputs"
 435     HI2LO
 436 THEN
 437 \ 3- select max of X,|Y|
 438 CMP X,T
 439 U< IF       \ X > |Y|
 440     MOV X,T
 441 THEN
 442 \ 4- abort if X or |Y| >= 16384
 443 CMP #16384,T
 444     U>= IF
 445     LO2HI
 446         ABORT" x or |y| >= 16384"
 447     HI2LO
 448     THEN
 449 \ 5- multiply inputs by 2^n scale factor
 450 MOV #1,S        \ init scale factor
 451 RLAM #3,T       \ test bit 2^13
 452 GOTO FW1
 453 BEGIN
 454     ADD X,X     \ X=X*2
 455     ADD Y,Y     \ Y=Y*2
 456     ADD S,S     \ scale factor *2
 457     ADD T,T     \ to test next bit 2^(n-1)
 458 FW1
 459 U>= UNTIL       \ until carry set
 460 \ 6- save IP and scale factor n
 461 PUSHM #2,IP     \ push IP,S
 462 \ ==================
 463 \ CORDIC engine
 464 \ ==================
 465 MOV #-1,IP          \ IP = i-1, X = Xi, Y = Yi
 466 MOV #0,TOS          \ init z=0
 467  BEGIN              \ i loops with init: i = 0
 468     ADD #1,IP
 469     MOV X,S         \ S = Xi to be right shifted
 470     MOV Y,T         \ T = Yi to be right shifted
 471     MOV #0,W        \ W = right shift loop count
 472     GOTO FW1
 473     BEGIN
 474         RRA S       \ (X >> i)
 475         RRA T       \ (Y >> i)
 476         ADD #1,W    \
 477 FW1     CMP IP,W    \ W = i ?
 478     0= UNTIL        \ 6~ loop
 479     ADD W,W         \ W = 2i = T_SCALE displacement
 480     CMP #0,Y        \ Y sign ?
 481     0>= IF          \ Y >= 0 : Rotate counter-clockwise
 482         ADD T,X     \ Xi+1 = Xi + ( Yi >> i)
 483         SUB S,Y     \ Yi+1 = Yi - ( Xi >> i)
 484         ADD T_ARCTAN(W),TOS
 485     ELSE            \ Y < 0 : Rotate clockwise
 486         SUB T,X     \ Xi+1 = Xi - ( Yi >> i)
 487         ADD S,Y     \ Yi+1 = Yi + ( Xi >> i)
 488         SUB T_ARCTAN(W),TOS
 489     THEN
 490     CMP #0,Y        \
 491     0<> WHILE       \ if Y = 0 quit loop ---+
 492     CMP #14,IP      \                       |
 493  0= UNTIL           \                       |
 494     THEN            \ <---------------------+
 495 \ multiply x by CORDIC gain
 496 MOV X,&MPY              \ 3     Load 1st operand
 497 MOV T_SCALE(W),&OP2     \ 3     CORDIC Gain * 65536
 498 MOV &RES1,X             \ 3     hi result = hypothenuse
 499 \ ==================
 500 \ endof CORDIC engine   \ X = hypothenuse, TOS = 256*angle
 501 \ ==================
 502 \ divide x by scale factor
 503 POPM #2,IP              \ S = scale factor, restore IP
 504 GOTO FW1
 505 BEGIN                   \ 4~ loop
 506     RRA X               \ divide x by 2
 507 FW1 RRA S               \ shift right scale factor
 508 U>= UNTIL               \ until carry set
 509 MOV X,0(PSP)
 510 \ multiply z by 256 to display it as a Q15.16 number
 511 MOV TOS,Y               \ Y = future fractional part of f
 512 SWPB TOS
 513 AND #$00FF,TOS
 514 SXT TOS                 \ integer part of f
 515 SWPB Y
 516 AND #$FF00,Y
 517 SUB #2,PSP
 518 MOV Y,0(PSP)            \ fractional part of f
 519 MOV @IP+,PC
 520 ENDCODE                 \
 521
 522 RST_HERE
 523
 524 [THEN]
 525
 526 [UNDEFINED] ROT [IF] \
 527 \ https://forth-standard.org/standard/core/ROT
 528 \ ROT    x1 x2 x3 -- x2 x3 x1
 529 CODE ROT
 530 MOV @PSP,W          \ 2 fetch x2
 531 MOV TOS,0(PSP)      \ 3 store x3
 532 MOV 2(PSP),TOS      \ 3 fetch x1
 533 MOV W,2(PSP)        \ 3 store x2
 534 MOV @IP+,PC
 535 ENDCODE
 536 [THEN]
 537
 538 : 1000CORDIC
 539 500 0 DO
 540     POL2REC REC2POL     \ 2 CORDIC op. * 500 loops = 1000 CORDIC
 541 LOOP
 542 ;
 543
 544 ECHO
 545
 546 ; -----------------------------------------------------------
 547 ; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
 548 ; -----------------------------------------------------------
 549
 550 \
 551 10000 89,0 POL2REC . .  ; sin, cos -->
 552 10000 75,0 POL2REC . .  ; sin, cos -->
 553 10000 60,0 POL2REC . .  ; sin, cos -->
 554 10000 45,0 POL2REC . .  ; sin, cos -->
 555 10000 30,0 POL2REC . .  ; sin, cos -->
 556 10000 15,0 POL2REC . .  ; sin, cos -->
 557 10000 1,0 POL2REC . .   ; sin, cos -->
 558 \ module phase -- X Y
 559 16384 30,0 POL2REC SWAP . . ; x, y -->
 560 16384 45,0 POL2REC SWAP . . ; x, y -->
 561 16384 60,0 POL2REC SWAP . . ; x, y -->
 562 \
 563
 564
 565 2  1  REC2POL F. .          ; phase module -->
 566 2 -1  REC2POL F. .          ; phase module -->
 567 20  10  REC2POL F. .        ; phase module -->
 568 20 -10  REC2POL F. .        ; phase module -->
 569 200 100 REC2POL F. .        ; phase module -->
 570 100 -100 REC2POL F. .       ; phase module -->
 571 2000 1000 REC2POL F. .      ; phase module -->
 572 1000 -1000 REC2POL F. .     ; phase module -->
 573 16000 8000 REC2POL F. .     ; phase module -->
 574 16000 -8000 REC2POL F. .    ; phase module -->
 575 16000 0 REC2POL F. .        ; phase module -->
 576 0 16000 REC2POL F. .        ; phase module -->
 577 \ 16384 -8192 REC2POL F. .    ; --> abort
 578 \ 0 0 REC2POL F. .            ; --> abort
 579
 580
 581 10000 89,0 POL2REC REC2POL   ROT . F.
 582 10000 75,0 POL2REC REC2POL   ROT . F.
 583 10000 60,0 POL2REC REC2POL   ROT . F.
 584 10000 45,0 POL2REC REC2POL   ROT . F.
 585 10000 30,0 POL2REC REC2POL   ROT . F.
 586 10000 26,565 POL2REC REC2POL ROT . F.
 587 10000 15,0 POL2REC REC2POL   ROT . F.
 588 10000 14,036 POL2REC REC2POL ROT . F.
 589 10000 7,125 POL2REC REC2POL  ROT . F.
 590 10000 1,0 POL2REC REC2POL    ROT . F.
 591
 592 10000 89,0   1000CORDIC      ROT . F.
 593 10000 75,0   1000CORDIC      ROT . F.
 594 10000 60,0   1000CORDIC      ROT . F.
 595 10000 45,0   1000CORDIC      ROT . F.
 596 10000 30,0   1000CORDIC      ROT . F.
 597 10000 26,565 1000CORDIC      ROT . F.
 598 10000 15,0   1000CORDIC      ROT . F.
 599 10000 14,036 1000CORDIC      ROT . F.
 600 10000 7,125  1000CORDIC      ROT . F.
 601 10000 1,0    1000CORDIC      ROT . F.
 602
 603