MSP430-FORTH/CORDIC.f

   1
   2 ; ----------
   3 ; CORDIC.f
   4 ; ----------
   5 \ see CORDICforDummies.pdf
   6 \
   7 \ to see kernel options, download FastForthSpecs.f
   8 \ FastForth kernel options: ASSEMBLER, CONDCOMP, FIXPOINT_INPUT.
   9 \
  10 \ TARGET Current Selection (used by preprocessor GEMA to load the pattern: \config\gema\TARGET.pat)
  11 \ MSP_EXP430FR5739  MSP_EXP430FR5969    MSP_EXP430FR5994    MSP_EXP430FR6989
  12 \ MSP_EXP430FR2433  MSP_EXP430FR2355    CHIPSTICK_FR2433
  13 \
  14 \ REGISTERS USAGE
  15 \ rDODOES to rEXIT must be saved before use and restored after
  16 \ scratch registers Y to S are free for use
  17 \ under interrupt, IP is free for use
  18 \
  19 \ PUSHM order : PSP,TOS, IP,  S,  T,  W,  X,  Y, rEXIT, rDOVAR, rDOCON, rDODOES
  20 \ example : PUSHM #6,IP pushes IP,S,T,W,X,Y registers to return stack
  21 \
  22 \ POPM  order :  rDODOES, rDOCON, rDOVAR, rEXIT,  Y,  X,  W,  T,  S, IP,TOS,PSP
  23 \ example : POPM #6,IP   pulls Y,X,W,T,S,IP registers from return stack
  24 \
  25 \ FORTH conditionnals:  unary{ 0= 0< 0> }, binary{ = < > U< }
  26 \
  27 \ ASSEMBLER conditionnal usage with IF UNTIL WHILE  S<  S>=  U<   U>=  0=  0<>  0>=
  28 \ ASSEMBLER conditionnal usage with ?JMP ?GOTO      S<  S>=  U<   U>=  0=  0<>  0<
  29
  30 [DEFINED] {CORDIC} [IF] {CORDIC} [THEN] \ remove {CORDIC}
  31
  32 MARKER {CORDIC}
  33
  34 [UNDEFINED] {FIXPOINT} [IF] \ define words to display angle as Q15.16 number.
  35
  36 \ https://forth-standard.org/standard/core/HOLDS
  37 \ Adds the string represented by addr u to the pictured numeric output string
  38 \ compilation use: <# S" string" HOLDS #>
  39 \ free chars area in the 32+2 bytes HOLD buffer sized for a 32 bits {hexa,decimal,binary} number = {26,23,2}.
  40 \ (2 supplementary bytes are room for sign - and decimal point)
  41 \ C HOLDS    addr u --
  42 CODE HOLDS
  43             MOV @PSP+,X     \ 2
  44             ADD TOS,X       \ 1 src
  45             MOV &HP,Y       \ 3 dst
  46 BEGIN       SUB #1,X        \ 1 src-1
  47             SUB #1,TOS      \ 1 cnt-1
  48 U>= WHILE   SUB #1,Y        \ 1 dst-1
  49             MOV.B @X,0(Y)   \ 4
  50 REPEAT      MOV Y,&HP       \ 3
  51             MOV @PSP+,TOS   \ 2
  52             MOV @IP+,PC     \ 4  15 words
  53 ENDCODE
  54
  55 \ F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number with u digits
  56 CODE F#S
  57             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
  58             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
  59             MOV X,0(PSP)            \ -- Qhi Qlo u
  60             MOV TOS,T               \                   T = limit
  61             MOV #0,S                \                   S = count
  62 BEGIN       MOV @PSP,&MPY           \                   Load 1st operand
  63             MOV &BASE,&OP2          \                   Load 2nd operand
  64             MOV &RES0,0(PSP)        \ -- Qhi RESlo x        low result on stack
  65             MOV &RES1,TOS           \ -- Qhi RESlo REShi    high result in TOS
  66             CMP #10,TOS             \                   digit to char
  67     U>= IF  ADD #7,TOS
  68     THEN    ADD #$30,TOS
  69             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
  70             ADD #1,S                \                   count+1
  71             CMP T,S                 \                   count=limit ?
  72 0= UNTIL    MOV #0,0(PSP)           \ -- Qhi 0 REShi
  73             MOV T,TOS               \ -- Qhi 0 limit
  74             SUB #2,PSP              \ -- Qhi 0 x len
  75             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
  76             JMP HOLDS
  77 ENDCODE
  78
  79 CODE F.             \ display a Q15.16 number with 4/5/16 digits after comma
  80 MOV TOS,S           \ S = sign
  81 MOV #4,T            \ T = 4     preset 4 digits for base 16 and by default
  82 MOV &BASE,W
  83 CMP ##10,W
  84 0= IF               \           if base 10
  85     ADD #1,T        \ T = 5     set 5 digits
  86 ELSE
  87     CMP #%10,W
  88     0= IF           \           if base 2
  89         MOV #16,T   \ T = 16    set 16 digits
  90     THEN
  91 THEN
  92 PUSHM #3,IP         \                   R-- IP sign #digit
  93 LO2HI
  94     <# DABS         \ -- uQlo uQhi      R-- IP sign #digit
  95     R> F#S          \ -- uQhi 0         R-- IP sign
  96     $2C HOLD        \                   $2C = char ','
  97     #S              \ -- 0 0
  98     R> SIGN #>      \ -- addr len       R-- IP
  99     TYPE SPACE      \ --
 100 ;
 101
 102 [THEN] \ end of [UNDEFINED] {FIXPOINT}
 103
 104 \ CORDIC USES
 105 \   OPERATION   |   MODE    |   INITIALIZE x y z    |   DIRECTION   |     RESULT        | post operation
 106 \ --------------|-----------|-----------------------|---------------|-------------------|
 107 \ sine, cosine  | Rotation  | x=1, y=0,  z=angle    | Reduce z to 0 | cos=x*Gi,sin=y*Gi | mutiply by 1/Gi
 108 \ --------------|-----------|-----------------------|---------------|-------------------|
 109 \ Polar to Rect | Rotation  | x=magnit, y=0, Z=angle| Reduce z to 0 |  X=x*Gi, Y=y*Gi   | mutiply by 1/Gi
 110 \ --------------|-----------|-----------------------|---------------|-------------------|
 111 \ Rotation      | Rotation  | x=X, y=Y, z=angle     | Reduce z to 0 | X'=x*Gi,Y'=y*Gi   | <=== not implemented
 112 \ --------------|-----------|-----------------------|---------------|-------------------|
 113 \ Rect to Polar |  Vector   | x=X, y=Y, z=0         | Reduce y to 0 | hyp=x*Gi, angle=z | mutiply hyp by 1/Gi
 114 \ --------------|-----------|-----------------------|---------------|-------------------|
 115 \ Gi = CORDIC gain for i iterations; Gi < 1
 116 \
 117
 118 CREATE T_ARCTAN \ ArcTan table
 119 11520 ,         \ 256 * 45
 120 6801 ,          \ 256 * 26.565
 121 3593 ,          \ 256 * 14.036
 122 1824 ,          \ 256 * 7.125
 123 916 ,           \ 256 * 3.576
 124 458 ,           \ 256 * 1.790
 125 229 ,           \ 256 * 0.895
 126 115 ,           \ 256 * 0.448
 127 57 ,            \ 256 * 0.224
 128 29 ,            \ 256 * 0.112
 129 14 ,            \ 256 * 0.056
 130 7 ,             \ 256 * 0.028
 131 4 ,             \ 256 * 0.014
 132 2 ,             \ 256 * 0.007
 133 1 ,             \ 256 * 0.003
 134
 135 CREATE T_SCALE  \ 1/Gi table
 136 46340 ,         \ = 65536 * cos(45)
 137 41448 ,         \ = 65536 * cos(45) * cos(26.565)
 138 40211 ,         \ = 65536 * cos(45) * cos(26.565) * cos(14.036)
 139 39900 ,         \ = 65536 * cos(45) * cos(26.565) * cos(14.036) * ...
 140 39822 ,
 141 39803 ,
 142 39798 ,
 143 39797 ,
 144 39797 ,
 145 39797 ,
 146 39797 ,
 147 39797 ,
 148 39797 ,
 149 39797 ,
 150 39797 ,
 151
 152
 153 CODE POL2REC   \ u f -- X Y
 154 \ input ; u = module {1000...16384}, f = angle (15Q16 number) in degrees {1,0...89,0}
 155 \ output ; X Y
 156 \ TOS = fhi, 0(PSP) = flo, 2(PSP) = u
 157 PUSH IP             \ save IP before use
 158 MOV @PSP+,Y         \ Y = flo
 159 SWPB Y
 160 AND #$00FF,Y
 161 SWPB TOS
 162 AND #$FF00,TOS
 163 BIS Y,TOS           \ -- module angle*256
 164 \ =====================
 165 \ CORDIC 16 bits engine
 166 \ =====================
 167 MOV #-1,IP          \ IP = i-1
 168 MOV @PSP,X          \ X = Xi
 169 MOV #0,Y            \ Y = Yi
 170  BEGIN              \ i loops with init i = 0
 171     ADD #1,IP
 172     MOV X,S         \ S = Xi to be right shifted
 173     MOV Y,T         \ T = Yi to be right shifted
 174     MOV #0,W        \
 175     GOTO FW1
 176     BEGIN
 177         RRA S       \ (Xi >> 1)
 178         RRA T       \ (Yi >> 1)
 179         ADD #1,W
 180     FW1 CMP IP,W    \ W = i ?
 181     0= UNTIL        \ loop back if W < i
 182     ADD W,W         \ W = 2i = T_SCALE displacement
 183     CMP #0,TOS      \ TOS = z
 184     0>= IF          \ TOS >= 0 : Rotate clockwise
 185         SUB T,X     \ Xi+1 = Xi - ( Yi >> i)
 186         ADD S,Y     \ Yi+1 = Yi + ( Xi >> i)
 187         SUB T_ARCTAN(W),TOS
 188     ELSE            \ TOS < 0 : Rotate counter-clockwise
 189         ADD T,X     \ Xi+1 = Xi + ( Yi >> i)
 190         SUB S,Y     \ Yi+1 = Yi - ( Xi >> i)
 191         ADD T_ARCTAN(W),TOS
 192     THEN
 193     CMP #0,TOS      \ if angle*256 = 0 quit loop
 194     0<> WHILE       \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
 195         CMP #14,IP
 196  0= UNTIL
 197     THEN            \ search "Extended control-flow patterns" in https://forth-standard.org/standard/rationale
 198 \ multiply cos by factor scale
 199 MOV X,&MPY              \ 3     Load 1st operand
 200 MOV T_SCALE(W),&OP2     \ 3     Load 2nd operand
 201 MOV &RES1,0(PSP)        \ 3     hi result = cos
 202 \ multiply sin by factor scale
 203 MOV Y,&MPY              \ 3     Load 1st operand
 204 MOV T_SCALE(W),&OP2     \ 3     Load 2nd operand
 205 MOV &RES1,TOS           \ 3     hi result = sin
 206 \ ==================
 207 \ endof CORDIC engine   \ X = cos, Y = sin
 208 \ ==================
 209 MOV @RSP+,IP
 210 MOV @IP+,PC
 211 ENDCODE                 \ -- cos sin
 212
 213
 214 \ REC2POL version with inputs scaling, to increase the accuracy of the angle:
 215 \ REC2POL   X Y -- u f
 216 \ input : X < 16384, |Y| < 16384
 217 \ output ; u = hypothenuse, f = angle (15Q16 number) in degrees
 218 \ rounded hypothenuse, 1 mn accuracy angle
 219 CODE REC2POL        \ X Y -- u f
 220 MOV @PSP,X          \ X = Xi
 221 MOV TOS,Y           \ Y = Yi
 222 \ normalize X Y to 16384 maxi
 223 \ 1- calculate T = |Y|
 224 MOV Y,T
 225 CMP #0,T
 226 S< IF
 227     XOR #-1,T
 228     ADD #1,T
 229 THEN
 230 \ 2- abort if null inputs
 231 MOV #-1,TOS \ set TOS TRUE for the two ABORT" below
 232 MOV X,S
 233 ADD T,S
 234 0= IF
 235     LO2HI
 236         ABORT" null inputs"
 237     HI2LO
 238 THEN
 239 \ 3- select max of X,|Y|
 240 CMP X,T
 241 U< IF       \ X > |Y|
 242     MOV X,T
 243 THEN
 244 \ 4- abort if X or |Y| >= 16384
 245 CMP #16384,T
 246     U>= IF
 247     LO2HI
 248         ABORT" x or |y| >= 16384"
 249     HI2LO
 250     THEN
 251 \ 5- multiply inputs by 2^n scale factor
 252 MOV #1,S        \ init scale factor
 253 RLAM #3,T       \ test bit 2^13
 254 GOTO FW1
 255 BEGIN
 256     ADD X,X     \ X=X*2
 257     ADD Y,Y     \ Y=Y*2
 258     ADD S,S     \ scale factor *2
 259     ADD T,T     \ to test next bit 2^(n-1)
 260 FW1
 261 U>= UNTIL       \ until carry set
 262 \ 6- save IP and scale factor n
 263 PUSHM #2,IP     \ push IP,S
 264 \ ==================
 265 \ CORDIC engine
 266 \ ==================
 267 MOV #-1,IP          \ IP = i-1, X = Xi, Y = Yi
 268 MOV #0,TOS          \ init z=0
 269  BEGIN              \ i loops with init: i = 0
 270     ADD #1,IP
 271     MOV X,S         \ S = Xi to be right shifted
 272     MOV Y,T         \ T = Yi to be right shifted
 273     MOV #0,W        \ W = right shift loop count
 274     GOTO FW1
 275     BEGIN
 276         RRA S       \ (X >> i)
 277         RRA T       \ (Y >> i)
 278         ADD #1,W    \
 279     FW1 CMP IP,W    \ W = i ?
 280     0= UNTIL        \ 6~ loop
 281     ADD W,W         \ W = 2i = T_SCALE displacement
 282     CMP #0,Y        \ Y sign ?
 283     0>= IF          \ Y >= 0 : Rotate counter-clockwise
 284         ADD T,X     \ Xi+1 = Xi + ( Yi >> i)
 285         SUB S,Y     \ Yi+1 = Yi - ( Xi >> i)
 286         ADD T_ARCTAN(W),TOS
 287     ELSE            \ Y < 0 : Rotate clockwise
 288         SUB T,X     \ Xi+1 = Xi - ( Yi >> i)
 289         ADD S,Y     \ Yi+1 = Yi + ( Xi >> i)
 290         SUB T_ARCTAN(W),TOS
 291     THEN
 292     CMP #0,Y        \ if Y = 0 quit loop
 293     0<> WHILE       \ if Y = 0 goto THEN
 294     CMP #14,IP
 295  0= UNTIL
 296     THEN
 297 \ multiply x by CORDIC gain
 298 MOV X,&MPY              \ 3     Load 1st operand
 299 MOV T_SCALE(W),&OP2     \ 3     CORDIC Gain * 65536
 300 MOV &RES1,X             \ 3     hi result = hypothenuse
 301 \ ==================
 302 \ endof CORDIC engine   \ X = hypothenuse, TOS = 256*angle
 303 \ ==================
 304 \ divide x by scale factor
 305 POPM #2,IP              \ S = scale factor, restore IP
 306 GOTO FW1
 307 BEGIN                   \ 4~ loop
 308     RRA X               \ divide x by 2
 309 FW1 RRA S               \ shift right scale factor
 310 U>= UNTIL               \ until carry set
 311 MOV X,0(PSP)
 312 \ multiply z by 256 to display it as a Q15.16 number
 313 MOV TOS,Y               \ Y = future fractional part of f
 314 SWPB TOS
 315 AND #$00FF,TOS
 316 SXT TOS                 \ integer part of f
 317 SWPB Y
 318 AND #$FF00,Y
 319 SUB #2,PSP
 320 MOV Y,0(PSP)            \ fractional part of f
 321 MOV @IP+,PC
 322 ENDCODE                 \
 323
 324 RST_HERE
 325
 326 : 2000CORDIC
 327 1000 0 DO
 328     POL2REC REC2POL     \ 1000 loops
 329 LOOP
 330 ;
 331
 332 ECHO
 333
 334 \
 335 10000 89,0 POL2REC . .  ; sin, cos -->
 336 10000 75,0 POL2REC . .  ; sin, cos -->
 337 10000 60,0 POL2REC . .  ; sin, cos -->
 338 10000 45,0 POL2REC . .  ; sin, cos -->
 339 10000 30,0 POL2REC . .  ; sin, cos -->
 340 10000 15,0 POL2REC . .  ; sin, cos -->
 341 10000 1,0 POL2REC . .   ; sin, cos -->
 342 \ module phase -- X Y
 343 16384 30,0 POL2REC SWAP . . ; x, y -->
 344 16384 45,0 POL2REC SWAP . . ; x, y -->
 345 16384 60,0 POL2REC SWAP . . ; x, y -->
 346 \
 347
 348
 349 2  1  REC2POL F. .          ; phase module -->
 350 2 -1  REC2POL F. .          ; phase module -->
 351 20  10  REC2POL F. .        ; phase module -->
 352 20 -10  REC2POL F. .        ; phase module -->
 353 200 100 REC2POL F. .        ; phase module -->
 354 100 -100 REC2POL F. .       ; phase module -->
 355 2000 1000 REC2POL F. .      ; phase module -->
 356 1000 -1000 REC2POL F. .     ; phase module -->
 357 16000 8000 REC2POL F. .     ; phase module -->
 358 16000 -8000 REC2POL F. .    ; phase module -->
 359 16000 0 REC2POL F. .        ; phase module -->
 360 0 16000 REC2POL F. .        ; phase module -->
 361 \16384 -8192 REC2POL F. .    ; --> abort
 362 \0 0 REC2POL F. .            ; --> abort
 363
 364
 365 10000 89,0 POL2REC REC2POL   ROT . F.
 366 10000 75,0 POL2REC REC2POL   ROT . F.
 367 10000 60,0 POL2REC REC2POL   ROT . F.
 368 10000 45,0 POL2REC REC2POL   ROT . F.
 369 10000 30,0 POL2REC REC2POL   ROT . F.
 370 10000 26,565 POL2REC REC2POL ROT . F.
 371 10000 15,0 POL2REC REC2POL   ROT . F.
 372 10000 14,036 POL2REC REC2POL ROT . F.
 373 10000 7,125 POL2REC REC2POL  ROT . F.
 374 10000 1,0 POL2REC REC2POL    ROT . F.
 375
 376 10000 89,0  2000CORDIC  ROT . F.
 377 10000 75,0  2000CORDIC  ROT . F.
 378 10000 60,0  2000CORDIC  ROT . F.
 379 10000 45,0  2000CORDIC  ROT . F.
 380 10000 30,0  2000CORDIC  ROT . F.
 381 10000 26,565 2000CORDIC ROT . F.
 382 10000 15,0 2000CORDIC   ROT . F.
 383 10000 14,036 2000CORDIC ROT . F.
 384 10000 7,125 2000CORDIC  ROT . F.
 385 10000 1,0 2000CORDIC    ROT . F.