MSP430-FORTH/SD_430FR5994/CORDIC.4TH

   1
   2     CODE ABORT_CORDIC
   3     SUB #4,R15
   4     MOV R14,2(R15)
   5     MOV &$180E,R14
   6     BIT #$100,R14
   7     0<> IF MOV #0,R14 THEN
   8     MOV R14,0(R15)
   9     MOV &$180A,R14
  10     SUB #309,R14
  11     COLON
  12     $0D EMIT
  13     ABORT" FastForth V3.9 please!"
  14     ABORT" build FastForth with FIXPOINT_INPUT addon"
  15     RST_RET
  16     ;
  17
  18     ABORT_CORDIC
  19
  20 ; ----------
  21 ; CORDIC.4th for MSP_EXP430FR5994
  22 ; ----------
  23
  24 MARKER {CORDIC}
  25
  26     CREATE T_ARCTAN
  27     12870 ,
  28     7598 ,
  29     4014 ,
  30     2038 ,
  31     1023 ,
  32     512 ,
  33     256 ,
  34     128 ,
  35     64 ,
  36     32 ,
  37     16 ,
  38     8 ,
  39     4 ,
  40     2 ,
  41     1 ,
  42
  43     CREATE T_SCALE
  44     46340 ,
  45     41448 ,
  46     40211 ,
  47     39900 ,
  48     39822 ,
  49     39803 ,
  50     39798 ,
  51     39797 ,
  52     39797 ,
  53     39797 ,
  54     39797 ,
  55     39797 ,
  56     39797 ,
  57     39797 ,
  58     39797 ,
  59
  60     [UNDEFINED] DABS
  61     [IF]
  62     CODE DABS
  63     AND #-1,R14
  64     S< IF
  65         XOR #-1,0(R15)
  66         XOR #-1,R14
  67         ADD #1,0(R15)
  68         ADDC #0,R14
  69     THEN
  70     MOV @R13+,R0
  71     ENDCODE
  72     [THEN]
  73
  74     [UNDEFINED] R>
  75     [IF]
  76     CODE R>
  77     SUB #2,R15
  78     MOV R14,0(R15)
  79     MOV @R1+,R14
  80     MOV @R13+,R0
  81     ENDCODE
  82     [THEN]
  83
  84     RST_SET
  85
  86     CODE TSTBIT
  87     MOV @R15+,R9
  88     AND @R9,R14
  89     MOV @R13+,R0
  90     ENDCODE
  91
  92     $180E 1 TSTBIT
  93
  94     RST_RET
  95
  96     [IF]   ; MSP430FRxxxx with hardware_MPY
  97
  98         [UNDEFINED] HOLDS
  99         [IF]
 100     CODE HOLDS
 101     MOV @R15+,R9
 102 BW3 ADD R14,R9
 103     MOV &$1DB2,R8
 104     BEGIN
 105        SUB #1,R9
 106         SUB #1,R14
 107     U>= WHILE
 108         SUB #1,R8
 109         MOV.B @R9,0(R8)
 110     REPEAT
 111     MOV R8,&$1DB2
 112     MOV @R15+,R14
 113     MOV @R13+,R0
 114     ENDCODE
 115         [THEN]
 116
 117         [UNDEFINED] F#S
 118         [IF]
 119     CODE F#S
 120                 MOV 2(R15),R9
 121                 MOV @R15,2(R15)
 122                 MOV R9,0(R15)
 123                 MOV R14,R11
 124                 MOV #0,R12
 125     BEGIN       MOV @R15,&$4C0
 126                 MOV &$1DBE,&$4C8
 127                 MOV &$4E4,0(R15)
 128                 MOV &$4E6,R14
 129                 CMP #10,R14
 130         U>= IF  ADD #7,R14
 131         THEN    ADD #$30,R14
 132                 MOV.B R14,$1D90(R12)
 133                 ADD #1,R12
 134                 CMP R11,R12
 135     0= UNTIL    MOV R11,R14
 136                 MOV #0,0(R15)
 137                 MOV #$1D90,R9
 138                 GOTO BW3
 139     ENDCODE
 140         [THEN]
 141
 142     HDNCODE XSCALE
 143     MOV T_SCALE(R10),&$4D4
 144     MOV #0,&$4D6
 145     MOV R9,&$4C8
 146     MOV &$4E6,R9
 147     MOV @R1+,R0
 148     ENDCODE
 149
 150     [ELSE] ; no hardware multiplier
 151
 152         [UNDEFINED] HOLDS
 153         [IF]
 154     CODE HOLDS
 155     MOV @R15+,R9
 156 BW3 ADD R14,R9
 157     MOV &$1DB2,R8
 158     BEGIN
 159        SUB #1,R9
 160         SUB #1,R14
 161     U>= WHILE
 162         SUB #1,R8
 163         MOV.B @R9,0(R8)
 164     REPEAT
 165     MOV R8,&$1DB2
 166     MOV @R15+,R14
 167     MOV @R13+,R0
 168     ENDCODE
 169         [THEN]
 170
 171         [UNDEFINED] F#S
 172         [IF]
 173     CODE F#S
 174                 MOV @R15,R12
 175                 MOV #0,R11
 176                 PUSHM #3,R13
 177                 MOV 2(R15),0(R15)
 178                 MOV R14,2(R15)
 179     BEGIN       MOV &$1DBE,R14
 180                 LO2HI
 181                 UM*
 182                 HI2LO
 183                 CMP #10,R14
 184         U>= IF  ADD #7,R14
 185         THEN    ADD #$30,R14
 186                 MOV @R1,R11
 187                 MOV.B R14,$1D90(R11)
 188                 ADD #1,R11
 189                 MOV R11,0(R1)
 190                 CMP 2(R15),R11
 191     U>= UNTIL   POPM #3,R13
 192                 MOV R11,R14
 193                 MOV R12,2(R15)
 194                 MOV #0,0(R15)
 195                 MOV #$1D90,R9
 196                 GOTO BW3
 197     ENDCODE
 198         [THEN]
 199
 200     HDNCODE XSCALE
 201                 MOV T_SCALE(R10),R6
 202     UMSTAR1     MOV #0,R8
 203                 MOV #0,R12
 204                 MOV #0,R11
 205                 MOV #1,R10
 206     BEGIN       BIT R10,R6
 207         0<> IF  ADD R9,R12
 208                 ADDC R8,R11
 209         THEN    ADD R9,R9
 210                 ADDC R8,R8
 211                 ADD R10,R10
 212     U>= UNTIL
 213                 MOV R11,R9
 214                 MOV #$40C4,R6
 215                 MOV @R1+,R0
 216     ENDCODE
 217
 218     [THEN]  ; endcase of hardware multiplier
 219
 220     CODE POL2REC
 221     PUSH R13
 222     MOV @R15+,&$4D0
 223     MOV R14,&$4D2
 224     MOV #286,&$4C8
 225     MOV &$4E4,R8
 226     MOV &$4E6,R14
 227     MOV #-1,R13
 228     MOV @R15,R9
 229     MOV #0,R8
 230     BEGIN
 231         ADD #1,R13
 232         MOV R9,R12
 233         MOV R8,R11
 234         MOV #0,R10
 235         GOTO FW1
 236         BEGIN
 237             RRA R12
 238             RRA R11
 239             ADD #1,R10
 240 FW1         CMP R13,R10
 241         0= UNTIL
 242         ADD R10,R10
 243         CMP #0,R14
 244         0>= IF
 245             SUB R11,R9
 246             ADD R12,R8
 247             SUB T_ARCTAN(R10),R14
 248         ELSE
 249             ADD R11,R9
 250             SUB R12,R8
 251             ADD T_ARCTAN(R10),R14
 252         THEN
 253         CMP #0,R14
 254         0<> WHILE
 255             CMP #14,R13
 256     0= UNTIL
 257         THEN
 258     CALL #XSCALE
 259     MOV R9,0(R15)
 260     MOV R8,R9
 261     CALL #XSCALE
 262     MOV R9,R14
 263     MOV @R1+,R13
 264     MOV @R13+,R0
 265     ENDCODE
 266
 267
 268     CODE REC2POL
 269     MOV @R15,R9
 270     MOV R14,R8
 271     MOV R8,R11
 272     CMP #0,R11
 273     S< IF
 274         XOR #-1,R11
 275         ADD #1,R11
 276     THEN
 277     MOV R9,R12
 278     CMP #0,R12
 279     S< IF
 280         XOR #-1,R12
 281         ADD #1,R12
 282     THEN
 283     MOV #-1,R14
 284     CMP #0,R9
 285     0= IF
 286         CMP #0,R8
 287         0= IF
 288             LO2HI
 289                 ABORT" null inputs!"
 290             HI2LO
 291         THEN
 292     THEN
 293     CMP R12,R11
 294     U< IF
 295         MOV R12,R11
 296     THEN
 297     CMP #16384,R11
 298         U>= IF
 299         LO2HI
 300             ABORT" |x| or |y| >= 16384"
 301         HI2LO
 302         THEN
 303     MOV #1,R12
 304     RLAM #3,R11
 305     GOTO FW1
 306     BEGIN
 307         ADD R9,R9
 308         ADD R8,R8
 309         ADD R12,R12
 310         ADD R11,R11
 311 FW1
 312     U>= UNTIL
 313     PUSHM #2,R13
 314     MOV #-1,R13
 315     MOV #0,R14
 316     BEGIN
 317         ADD #1,R13
 318         MOV R9,R12
 319         MOV R8,R11
 320         MOV #0,R10
 321         GOTO FW1
 322         BEGIN
 323             RRA R12
 324             RRA R11
 325             ADD #1,R10
 326 FW1         CMP R13,R10
 327         0= UNTIL
 328         ADD R10,R10
 329         CMP #0,R8
 330         S>= IF
 331             ADD R11,R9
 332             SUB R12,R8
 333             ADD T_ARCTAN(R10),R14
 334         ELSE
 335             SUB R11,R9
 336             ADD R12,R8
 337             SUB T_ARCTAN(R10),R14
 338         THEN
 339         CMP #0,R8
 340         0<> WHILE
 341         CMP #14,R13
 342     0= UNTIL
 343         THEN
 344     CALL #XSCALE
 345     POPM #2,R13
 346     GOTO FW1
 347     BEGIN
 348         RRA R9
 349 FW1     RRA R12
 350     U>= UNTIL
 351     MOV R9,0(R15)
 352     SUB #4,R15
 353     MOV R14,R6
 354     CMP #0,R6
 355     S< IF
 356         XOR #-1,R14
 357         ADD #1,R14
 358     THEN
 359     MOV #0,2(R15)
 360     MOV R14,0(R15)
 361     MOV #286,R14
 362     CALL #$403E
 363     MOV @R15+,0(R15)
 364     CMP #0,R6
 365     S< IF
 366         XOR #-1,0(R15)
 367         XOR #-1,R14
 368         ADD #1,0(R15)
 369         ADDC #0,R14
 370     THEN
 371     MOV #$40C4,R6
 372     MOV @R13+,R0
 373     ENDCODE
 374
 375
 376     [UNDEFINED] F.
 377     [IF]
 378     CODE F.
 379     MOV R14,R12
 380     MOV #4,R11
 381     MOV &$1DBE,R10
 382     CMP ##10,R10
 383     0= IF
 384         ADD #1,R11
 385     ELSE
 386         CMP #%10,R10
 387         0= IF
 388             MOV #16,R11
 389         THEN
 390     THEN
 391     PUSHM #3,R13
 392     LO2HI
 393         <# DABS
 394         R> F#S
 395         $2C HOLD
 396         #S
 397         R> SIGN #>
 398         TYPE $20 EMIT
 399     ;
 400
 401     [THEN]
 402
 403 RST_SET
 404
 405     [UNDEFINED] SWAP
 406     [IF]
 407     CODE SWAP
 408     MOV @R15,R10
 409     MOV R14,0(R15)
 410     MOV R10,R14
 411     MOV @R13+,R0
 412     ENDCODE
 413     [THEN]
 414
 415     [UNDEFINED] ROT
 416     [IF]
 417     CODE ROT
 418     MOV @R15,R10
 419     MOV R14,0(R15)
 420     MOV 2(R15),R14
 421     MOV R10,2(R15)
 422     MOV @R13+,R0
 423     ENDCODE
 424     [THEN]
 425
 426 ECHO
 427
 428 10000 89,0 POL2REC . .  ; sin, cos -->
 429 10000 75,0 POL2REC . .  ; sin, cos -->
 430 10000 60,0 POL2REC . .  ; sin, cos -->
 431 10000 45,0 POL2REC . .  ; sin, cos -->
 432 10000 30,0 POL2REC . .  ; sin, cos -->
 433 10000 15,0 POL2REC . .  ; sin, cos -->
 434 10000 1,0 POL2REC . .   ; sin, cos -->
 435 16384 30,0 POL2REC SWAP . . ; x, y -->
 436 16384 45,0 POL2REC SWAP . . ; x, y -->
 437 16384 60,0 POL2REC SWAP . . ; x, y -->
 438
 439 10000 -89,0 POL2REC . .  ; sin, cos -->
 440 10000 -75,0 POL2REC . .  ; sin, cos -->
 441 10000 -60,0 POL2REC . .  ; sin, cos -->
 442 10000 -45,0 POL2REC . .  ; sin, cos -->
 443 10000 -30,0 POL2REC . .  ; sin, cos -->
 444 10000 -15,0 POL2REC . .  ; sin, cos -->
 445 10000 -1,0 POL2REC . .   ; sin, cos -->
 446 16384 -30,0 POL2REC SWAP . . ; x, y -->
 447 16384 -45,0 POL2REC SWAP . . ; x, y -->
 448 16384 -60,0 POL2REC SWAP . . ; x, y -->
 449
 450 -10000 89,0 POL2REC . .  ; sin, cos -->
 451 -10000 75,0 POL2REC . .  ; sin, cos -->
 452 -10000 60,0 POL2REC . .  ; sin, cos -->
 453 -10000 45,0 POL2REC . .  ; sin, cos -->
 454 -10000 30,0 POL2REC . .  ; sin, cos -->
 455 -10000 15,0 POL2REC . .  ; sin, cos -->
 456 -10000 1,0 POL2REC . .   ; sin, cos -->
 457 -16384 30,0 POL2REC SWAP . . ; x, y -->
 458 -16384 45,0 POL2REC SWAP . . ; x, y -->
 459 -16384 60,0 POL2REC SWAP . . ; x, y -->
 460
 461 -10000 -89,0 POL2REC . .  ; sin, cos -->
 462 -10000 -75,0 POL2REC . .  ; sin, cos -->
 463 -10000 -60,0 POL2REC . .  ; sin, cos -->
 464 -10000 -45,0 POL2REC . .  ; sin, cos -->
 465 -10000 -30,0 POL2REC . .  ; sin, cos -->
 466 -10000 -15,0 POL2REC . .  ; sin, cos -->
 467 -10000 -1,0 POL2REC . .   ; sin, cos -->
 468 -16384 -30,0 POL2REC SWAP . . ; x, y -->
 469 -16384 -45,0 POL2REC SWAP . . ; x, y -->
 470 -16384 -60,0 POL2REC SWAP . . ; x, y -->
 471
 472
 473 2  1  REC2POL F. .          ; phase module -->
 474 2 -1  REC2POL F. .          ; phase module -->
 475 20  10  REC2POL F. .        ; phase module -->
 476 20 -10  REC2POL F. .        ; phase module -->
 477 200 100 REC2POL F. .        ; phase module -->
 478 100 -100 REC2POL F. .       ; phase module -->
 479 2000 1000 REC2POL F. .      ; phase module -->
 480 1000 -1000 REC2POL F. .     ; phase module -->
 481 16000 8000 REC2POL F. .     ; phase module -->
 482 16000 -8000 REC2POL F. .    ; phase module -->
 483 16000 0 REC2POL F. .        ; phase module -->
 484 0 16000 REC2POL F. .        ; phase module -->
 485
 486 -2  1  REC2POL F. .          ; phase module -->
 487 -2 -1  REC2POL F. .          ; phase module -->
 488 -20  10  REC2POL F. .        ; phase module -->
 489 -20 -10  REC2POL F. .        ; phase module -->
 490 -200 100 REC2POL F. .        ; phase module -->
 491 -100 -100 REC2POL F. .       ; phase module -->
 492 -2000 1000 REC2POL F. .      ; phase module -->
 493 -1000 -1000 REC2POL F. .     ; phase module -->
 494 -16000 8000 REC2POL F. .     ; phase module -->
 495 -16000 -8000 REC2POL F. .    ; phase module -->
 496 16000 0 REC2POL F. .        ; phase module -->
 497 0 16000 REC2POL F. .        ; phase module -->
 498
 499 10000 89,0 POL2REC REC2POL   ROT . F.
 500 10000 75,0 POL2REC REC2POL   ROT . F.
 501 10000 60,0 POL2REC REC2POL   ROT . F.
 502 10000 45,0 POL2REC REC2POL   ROT . F.
 503 10000 30,0 POL2REC REC2POL   ROT . F.
 504 10000 26,565 POL2REC REC2POL ROT . F.
 505 10000 15,0 POL2REC REC2POL   ROT . F.
 506 10000 14,036 POL2REC REC2POL ROT . F.
 507 10000 7,125 POL2REC REC2POL  ROT . F.
 508 10000 1,0 POL2REC REC2POL    ROT . F.
 509
 510