MSP430-FORTH/MSP_EXP430FR5994/CORDIC.4TH

   1
   2 ; ----------
   3 ; CORDIC.4th
   4 ; ----------
   5
   6 PWR_STATE
   7
   8 : DEFINED! ECHO 1 ABORT" already loaded!" ;
   9
  10 [DEFINED] {CORDIC} [IF] DEFINED!
  11
  12 [ELSE]
  13
  14 MARKER {CORDIC}
  15
  16 [UNDEFINED] {FIXPOINT} [IF]
  17
  18 CODE HOLDS
  19             MOV @R15+,R9
  20             ADD R14,R9
  21             MOV &$1DB2,R8
  22 BEGIN       SUB #1,R9
  23             SUB #1,R14
  24 U>= WHILE   SUB #1,R8
  25             MOV.B @R9,0(R8)
  26 REPEAT      MOV R8,&$1DB2
  27             MOV @R15+,R14
  28             MOV @R13+,R0
  29 ENDCODE
  30
  31 CODE F#S
  32             MOV 2(R15),R9
  33             MOV @R15,2(R15)
  34             MOV R9,0(R15)
  35             MOV R14,R11
  36             MOV #0,R12
  37 BEGIN       MOV @R15,&$4C0
  38             MOV &BASE,&$4C8
  39             MOV &$4E4,0(R15)
  40             MOV &$4E6,R14
  41             CMP #10,R14
  42     U>= IF  ADD #7,R14
  43     THEN    ADD #$30,R14
  44             MOV.B R14,$1D90(R12)
  45             ADD #1,R12
  46             CMP R11,R12
  47 0= UNTIL    MOV #0,0(R15)
  48             MOV R11,R14
  49             SUB #2,R15
  50             MOV #$1D90,0(R15)
  51             JMP HOLDS
  52 ENDCODE
  53
  54 CODE F.
  55 MOV R14,R12
  56 MOV #4,R11
  57 MOV &BASE,R10
  58 CMP ##10,R10
  59 0= IF
  60     ADD #1,R11
  61 ELSE
  62     CMP #%10,R10
  63     0= IF
  64         MOV #16,R11
  65     THEN
  66 THEN
  67 PUSHM #3,R13
  68 LO2HI
  69     <# DABS
  70     R> F#S
  71     $2C HOLD
  72     #S
  73     R> SIGN #>
  74     TYPE SPACE
  75 ;
  76
  77 [THEN]
  78
  79
  80 CREATE T_ARCTAN
  81 11520 ,
  82 6801 ,
  83 3593 ,
  84 1824 ,
  85 916 ,
  86 458 ,
  87 229 ,
  88 115 ,
  89 57 ,
  90 29 ,
  91 14 ,
  92 7 ,
  93 4 ,
  94 2 ,
  95 1 ,
  96
  97 CREATE T_SCALE
  98 46340 ,
  99 41448 ,
 100 40211 ,
 101 39900 ,
 102 39822 ,
 103 39803 ,
 104 39798 ,
 105 39797 ,
 106 39797 ,
 107 39797 ,
 108 39797 ,
 109 39797 ,
 110 39797 ,
 111 39797 ,
 112 39797 ,
 113
 114
 115 CODE POL2REC
 116 PUSH R13
 117 MOV @R15+,R8
 118 SWPB R8
 119 AND #$00FF,R8
 120 SWPB R14
 121 AND #$FF00,R14
 122 BIS R8,R14
 123 MOV #-1,R13
 124 MOV @R15,R9
 125 MOV #0,R8
 126  BEGIN
 127     ADD #1,R13
 128     MOV R9,R12
 129     MOV R8,R11
 130     MOV #0,R10
 131     GOTO FW1
 132     BEGIN
 133         RRA R12
 134         RRA R11
 135         ADD #1,R10
 136     FW1 CMP R13,R10
 137     0= UNTIL
 138     ADD R10,R10
 139     CMP #0,R14
 140     0>= IF
 141         SUB R11,R9
 142         ADD R12,R8
 143         SUB T_ARCTAN(R10),R14
 144     ELSE
 145         ADD R11,R9
 146         SUB R12,R8
 147         ADD T_ARCTAN(R10),R14
 148     THEN
 149     CMP #0,R14
 150     0<> WHILE
 151         CMP #14,R13
 152  0= UNTIL
 153     THEN
 154 MOV R9,&$4C0
 155 MOV T_SCALE(R10),&$4C8
 156 MOV &$4E6,0(R15)
 157 MOV R8,&$4C0
 158 MOV T_SCALE(R10),&$4C8
 159 MOV &$4E6,R14
 160 MOV @R1+,R13
 161 MOV @R13+,R0
 162 ENDCODE
 163
 164
 165 CODE REC2POL
 166 MOV @R15,R9
 167 MOV R14,R8
 168 MOV R8,R11
 169 CMP #0,R11
 170 S< IF
 171     XOR #-1,R11
 172     ADD #1,R11
 173 THEN
 174 MOV #-1,R14
 175 MOV R9,R12
 176 ADD R11,R12
 177 0= IF
 178     LO2HI
 179         ABORT" null inputs"
 180     HI2LO
 181 THEN
 182 CMP R9,R11
 183 U< IF
 184     MOV R9,R11
 185 THEN
 186 CMP #16384,R11
 187     U>= IF
 188     LO2HI
 189         ABORT" x or |y| >= 16384"
 190     HI2LO
 191     THEN
 192 MOV #1,R12
 193 RLAM #3,R11
 194 GOTO FW1
 195 BEGIN
 196     ADD R9,R9
 197     ADD R8,R8
 198     ADD R12,R12
 199     ADD R11,R11
 200 FW1
 201 U>= UNTIL
 202 PUSHM #2,R13
 203 MOV #-1,R13
 204 MOV #0,R14
 205  BEGIN
 206     ADD #1,R13
 207     MOV R9,R12
 208     MOV R8,R11
 209     MOV #0,R10
 210     GOTO FW1
 211     BEGIN
 212         RRA R12
 213         RRA R11
 214         ADD #1,R10
 215     FW1 CMP R13,R10
 216     0= UNTIL
 217     ADD R10,R10
 218     CMP #0,R8
 219     0>= IF
 220         ADD R11,R9
 221         SUB R12,R8
 222         ADD T_ARCTAN(R10),R14
 223     ELSE
 224         SUB R11,R9
 225         ADD R12,R8
 226         SUB T_ARCTAN(R10),R14
 227     THEN
 228     CMP #0,R8
 229     0<> WHILE
 230     CMP #14,R13
 231  0= UNTIL
 232     THEN
 233 MOV R9,&$4C0
 234 MOV T_SCALE(R10),&$4C8
 235 MOV &$4E6,R9
 236 POPM #2,R13
 237 GOTO FW1
 238 BEGIN
 239     RRA R9
 240 FW1 RRA R12
 241 U>= UNTIL
 242 MOV R9,0(R15)
 243 MOV R14,R8
 244 SWPB R14
 245 AND #$00FF,R14
 246 SXT R14
 247 SWPB R8
 248 AND #$FF00,R8
 249 SUB #2,R15
 250 MOV R8,0(R15)
 251 MOV @R13+,R0
 252 ENDCODE
 253
 254 RST_HERE
 255
 256 [THEN]
 257
 258 : 2000CORDIC
 259 1000 0 DO
 260     POL2REC REC2POL
 261 LOOP
 262 ;
 263
 264 ECHO
 265
 266 10000 89,0 POL2REC . .  ; sin, cos -->
 267 10000 75,0 POL2REC . .  ; sin, cos -->
 268 10000 60,0 POL2REC . .  ; sin, cos -->
 269 10000 45,0 POL2REC . .  ; sin, cos -->
 270 10000 30,0 POL2REC . .  ; sin, cos -->
 271 10000 15,0 POL2REC . .  ; sin, cos -->
 272 10000 1,0 POL2REC . .   ; sin, cos -->
 273 16384 30,0 POL2REC SWAP . . ; x, y -->
 274 16384 45,0 POL2REC SWAP . . ; x, y -->
 275 16384 60,0 POL2REC SWAP . . ; x, y -->
 276
 277
 278 2  1  REC2POL F. .          ; phase module -->
 279 2 -1  REC2POL F. .          ; phase module -->
 280 20  10  REC2POL F. .        ; phase module -->
 281 20 -10  REC2POL F. .        ; phase module -->
 282 200 100 REC2POL F. .        ; phase module -->
 283 100 -100 REC2POL F. .       ; phase module -->
 284 2000 1000 REC2POL F. .      ; phase module -->
 285 1000 -1000 REC2POL F. .     ; phase module -->
 286 16000 8000 REC2POL F. .     ; phase module -->
 287 16000 -8000 REC2POL F. .    ; phase module -->
 288 16000 0 REC2POL F. .        ; phase module -->
 289 0 16000 REC2POL F. .        ; phase module -->
 290
 291
 292 10000 89,0 POL2REC REC2POL   ROT . F.
 293 10000 75,0 POL2REC REC2POL   ROT . F.
 294 10000 60,0 POL2REC REC2POL   ROT . F.
 295 10000 45,0 POL2REC REC2POL   ROT . F.
 296 10000 30,0 POL2REC REC2POL   ROT . F.
 297 10000 26,565 POL2REC REC2POL ROT . F.
 298 10000 15,0 POL2REC REC2POL   ROT . F.
 299 10000 14,036 POL2REC REC2POL ROT . F.
 300 10000 7,125 POL2REC REC2POL  ROT . F.
 301 10000 1,0 POL2REC REC2POL    ROT . F.
 302
 303 10000 89,0  2000CORDIC  ROT . F.
 304 10000 75,0  2000CORDIC  ROT . F.
 305 10000 60,0  2000CORDIC  ROT . F.
 306 10000 45,0  2000CORDIC  ROT . F.
 307 10000 30,0  2000CORDIC  ROT . F.
 308 10000 26,565 2000CORDIC ROT . F.
 309 10000 15,0 2000CORDIC   ROT . F.
 310 10000 14,036 2000CORDIC ROT . F.
 311 10000 7,125 2000CORDIC  ROT . F.
 312 10000 1,0 2000CORDIC    ROT . F.
 313
 314