MSP430-FORTH/CORDIC.f

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