MSP430-FORTH/CORDIC.f

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