         Edinburgh IMP77 Compiler - Version 8.4

    1  ! This is effectively a fixed-point implementation of CORDIC sin and cos which
    2  ! checks results against the floating-point library used by C. There are a few
    3  ! things we could do to improve accuracy - but this is good enough for a quick
    4  ! hack that does not use any external math library code, other than the afore-
    5  ! mentioned consistency checks.
    6  
    7  ! The program is based on Michael Bertrand's CORDIC article which has appeared
    8  ! in multiple places including Dr Dobbs magazine, a copy of which can be found
    9  ! at http://www.pennelynn.com/Documents/CUJ/HTML/92HTML/1992024C.HTM
   10  
   11  ! Note that in creating this implementation, we hit a bug that manifested at
   12  ! the the extremes of the sin and cos functions (where the values are close
   13  ! to 0 or 1) and which I have patched with an approximation since the functions
   14  ! are close to linear at those points.  However the hack to correct that error
   15  ! does make this code a little larger than the C version from which it was
   16  ! derived.
   17  
   18  ! I have thrown in a local implementation of atan and sqrt for good measure :-)
   19  
   20  ! Imp77 does not have unsigned integer data types; however since the CORDIC code
   21  ! effectively uses a 1.14 fixed point implementation to implement angles (with
   22  ! the original C sources being compatible with a 16-bit int environment), and
   23  ! the current Imp77 compiler being a 32 bit implementation, unsigned integers
   24  ! have not required.
   25  
   26  %begin
   27  
   28  %const %integer debug = 1 { 0 : 4 }
   29  
   30  !%external %real %fn %spec atan %alias "atanf" (%real angle)  ;! borrow C's math library for use in demo.
   31  %real %fn atan(%longreal x1)
   32    %const %long %real r1= 1.2595802263029547 @  1
   33    %const %long %real r2=-8.6186317517509520 @  1
   34    %const %long %real r3=-1.2766919133361079 @  0
   35    %const %long %real r4=-8.3921038065840512 @ -2
   36    %const %long %real s1= 2.7096164294378656 @  1
   37    %const %long %real s2= 6.5581320451487386 @  0
   38    %const %long %real s3= 2.1441643116703661 @  0
   39    %const %long %real s4= 1.2676256708212610 @  0
   40    %const %long %real rt3= 1.7320508075688772 @  0
   41    %const %long %real piby6= 5.2359877559829887 @ -1
   42    %const %long %real piby2m1= 5.7079632679489661 @ -1
   43    %const %long %real rt3m1=7.3205080756887728 @ -1
   44    %const %long %real tanpiby12= 2.6794919243112271 @ -1
   45    %long %real xx1,xsq,constant=0
   46    %integer sign=0,inv=0
   47    
   48    %if x1<0 %then sign=1 %and xx1=-x1 %else xx1=x1
   49    %if xx1>1.0 %then xx1=1.0/xx1 %and inv=1
   50    %if xx1>tanpiby12 %then xx1=(rt3m1*xx1-1.0+xx1)/(xx1+rt3) %and constant=piby6
   51    xsq=xx1*xx1
   52    xx1 = xx1*(r1/(xsq+s1+r2/(xsq+s2+r3/(xsq+s3+r4/(xsq+s4)))))
   53    xx1 = xx1 + constant
   54    %if inv=1 %then xx1=1.0-xx1+piby2m1
   55    %if sign=1 %then xx1=-xx1
   56    %result=xx1 { shorten from %long %real down to %real when returning result }
   57  %end
   58  
   59  !%external %real %fn %spec sqrt %alias "sqrtf" (%real angle)
   60  %real %fn sqrt(%long %real X)
   61    %integer %fn approx integer sqrt(%integer X)
   62      %integer k = 1, result = 1
   63      k = k+1 %and result = k*k %while result <= X
   64      %result = k-1 {note 'off by 1' bug in original C version due to returning k--}
   65    %end
   66    %integer I
   67    %long %real prev A, A, epsilon = X / 100  {accurate enough due to .14 fixed point}
   68    %if X = 2.0 %then %result = 1.41421356237 %else I = approx integer sqrt(INT PT(X))
   69    %result = I %if X = I*I { exact root }
   70    ! Newton's method: A{n} = (A{n-1} + X/A{n-1})/2
   71    A = I; prev A = A %and A = (A + X/A)/2.0 %until prev A - epsilon <= A %and A <= prev A + epsilon
   72    %result = A
   73  %end
   74  
   75  %real PI = atan(1) * 4
   76  
   77  %const %integer K64 = 1<<16, K32 = K64>>1, K16 = K32>>1, K8  = K16>>1
   78  
   79  %routine phex(%integer x)
   80    %own %string (16) hex = "0123456789ABCDEF"
   81    %integer i
   82    print symbol(charno(hex, (x >> (i<<2))&15+1)) %for i = 7, -1, 0
   83  %end
   84  
   85  %routine SinCos({unsigned} %integer theta, %integer %name sin, %integer %name cos)
   86    %const %integer QUAD1 = 3, QUAD2 = 2, QUAD3 = 0, QUAD4 = 1   { bit 1 for COS x vs -x,  bit 2 for SIN x vs -x }
   87    %const %integer NBITS = 14                                   { NBITS is number of bits for CORDIC units. }
   88  
   89    {unsigned} %const %integer CordicBase = 1 << NBITS                      { base for CORDIC units }
   90    {unsigned} %const %integer HalfBase = CordicBase >> 1                   { CordicBase / 2 }
   91    {unsigned} %const %integer Quad2Boundary = CordicBase << 1              { 2 * CordicBase }
   92    {unsigned} %const %integer Quad3Boundary = CordicBase + Quad2Boundary   { 3 * CordicBase }
   93  
   94    ! these owns are initialised on the first call to SinCos:
   95    %own %integer %array ArcTan(0:NBITS-1)                       { angles for rotation }
   96    %own %integer xInit                                          { initial x projection }
   97    
   98    %integer xcos,xsin  { used to patch a problem that happens near the zeroes and extremes of the results }
   99    
  100    !  USE : Calc sin and cos with integer CORDIC routine.
  101    !  IN  : theta = incoming angle (in CORDIC angle units)
  102    !  OUT : sin (in CORDIC fixed point units)
  103    !        cos (in CORDIC fixed point units)
  104    !  NOTE: The incoming angle theta is in CORDIC angle
  105    !        units, which subdivide the circle into 64K parts,
  106    !        with 0 deg = 0, 90 deg = 16384 (CordicBase), 180 deg
  107    !        = 32768, 270 deg = 49152, etc. The calculated sine
  108    !        and cosine are in CORDIC fixed point units - an int
  109    !        considered as a fraction of 16384 (CordicBase).
  110  
  111    %integer quadrant  { quadrant of incoming angle }
  112    %integer z         { incoming angle moved to 1st quad }
  113    %integer i         { to index rotations - one per bit }
  114    %integer x, y      { projections onto axes }
  115    %integer xl, yl    { projections of rotated vector }
  116  
  117    %own %integer initialised = 0
  118    %if initialised = 0 %start
  119  
  120      !   USE : Load globals used by SinCos().
  121      !   OUT : Loads globals used in SinCos() :
  122      !    CordicBase    = base for CORDIC units
  123      !    HalfBase      = Cordicbase / 2
  124      !    Quad2Boundary = 2 * CordicBase
  125      !    Quad3Boundary = 3 * CordicBase
  126      !    ArcTan()      = the arctans of 1/(2^i)
  127      !    xInit         = initial value for x projection
  128      !   NOTE: Must be called once only to initialize before
  129      !    calling SinCos(). xInit is sufficiently less than
  130      !    CordicBase to exactly compensate for the expansion
  131      !    in each rotation.
  132    
  133      {%integer i}      { to index ArcTan() }
  134      %real f           { to calc initial x projection }
  135      %integer powr     { powers of 2 up to 2^(2*(NBITS-1)) }
  136    
  137      { ArcTans are diminishingly small angles. }
  138      powr = 1
  139      %for i = 0, 1, NBITS-1 %cycle
  140      
  141        ! We could alternatively have created ArcTan as an array of constants at compile time.
  142        ArcTan(i) = INT(atan(1.0/powr)/(PI/2.0)*CordicBase)
  143        %if debug > 1 %start
  144          print string("Setting ArcTan("); write(i,0); print string(") to "); phex(ArcTan(i)); newline
  145        %finish
  146        powr = powr << 1
  147        ! xInit is initial value of x projection to compensate for expansions. f = 1/sqrt(2/1 * 5/4 * ...
  148        ! Normalize as an NBITS binary fraction (multiply by CordicBase) and store in xInit.
  149        ! Get f = 0.607253 and xInit = 9949 = 0x26DD for NBITS = 14.
  150      %repeat
  151      f = 1.0
  152  
  153      powr = 1
  154      %for i = 0, 1, NBITS-1 %cycle
  155        f = (f * (powr + 1)) / powr
  156        powr = powr << 2
  157      %repeat
  158  
  159      f = 1.0/sqrt(f);              { sqrt is expensive but only done once during initialisation }
  160                                    { Note could have implemented f = rsqrt(f) instead and been faster }
  161                                    
  162      xInit = INT(CordicBase * f)   { rounding type conversion to fixed point }
  163      %if debug # 0 %start
  164        print string("xInit = "); print fl(CordicBase * f, 12)
  165        print string(" = "); write(xInit, 0); newlines(2)
  166      %finish
  167      
  168      initialised = 1
  169    %finish  { initialisation }
  170  
  171    ! Determine quadrant of incoming angle, translate to 1st quadrant.
  172    ! Note use of previously calculated values CordicBase, etc. for speed.
  173  
  174    theta = theta & (K64-1)  { reduce to a single circle }
  175    %if       theta < CordicBase    %then quadrant = QUAD1 %and z = theta                 %c
  176+   %else %if theta < Quad2Boundary %then quadrant = QUAD2 %and z = Quad2Boundary - theta %c
  177+   %else %if theta < Quad3Boundary %then quadrant = QUAD3 %and z = theta - Quad2Boundary %c
  178+   %else                                 quadrant = QUAD4 %and z = -theta
  179  
  180    ! Initialize projections; Negate z, so same rotations taking angle z to 0 will take (x, y) = (xInit, 0) to (cos, sin).
  181    z = z & (K32-1) { this is a bit dodgy on my part as there *are* cases where z is negative above }
  182    x = xInit; y = 0; z = -z
  183  
  184    %for i = 0, 1, NBITS-1 %cycle                                                    { Rotate NBITS times. }
  185      %if z < 0 %then z = z+ArcTan(i) %and yl = y + (x >> i) %and xl = x - (y >> i)  { Counter-clockwise rotation. } %c
  186+     %else z = z-ArcTan(i) %and yl = y - (x >> i) %and xl = x + (y >> i)            { Clockwise rotation. }
  187      x = xl; y = yl                                                                 { Put new projections into (x,y) for next go. }
  188    %repeat
  189  
  190    %if quadrant&1 # 0 %then cos = x %else cos = -x  { Attach signs depending on quadrant. }
  191    %if quadrant&4 # 0 %then sin = y %else sin = -y
  192  
  193    xsin=sin {debug}
  194    sin = -sin %if Theta < K32
  195    sin = sin & (K64-1); sin = sin!(\(K16-1)) %if sin > K16
  196  
  197    xcos=cos {debug}
  198    cos = cos & (K64-1); cos = cos!(\(K16-1)) %if cos > K16
  199  
  200    %if debug > 1 %start
  201      print string("                  Theta ="); write(theta*1608//1024,8)
  202      print string("; SIN(Theta) = "); phex(xsin);
  203      print string("; COS(Theta) = "); phex(xcos);
  204      newline
  205    %finish
  206  
  207    ! There is a bug in this implementation that fails near multiples of 90 degrees -
  208    ! this test catches the broken cases and uses an approximation to repair the results :-(
  209    ! I will remove these hacks later, if I ever find and fix the bug properly!
  210    ! My suspicion is of overflow of a partial 16x16 multiplication somewhere, or maybe a '<<'.
  211    %if xsin&16_FFFFC000 # 16_FFFFC000 %and xsin&16_FFFFC000 # 0 %then %start
  212      %if                  theta < 16_0400 %start
  213        sin = theta*1608//1024
  214      %else %if 16_3C00 <= theta < 16_4000
  215        sin = K16 - (16_4000-theta)//25
  216      %else %if 16_4000 <= theta < 16_4400
  217        sin = K16 - (theta-16_4000)//25
  218      %else %if 16_7C00 <= theta < 16_8400
  219        sin = (16_8000-theta)*1608//1024
  220      %else %if 16_BC00 <= theta < 16_C000
  221        sin = -K16 + (16_C000-theta)//25
  222      %else %if 16_C000 <= theta < 16_C400
  223        sin = -K16 + (theta-16_C000)//25
  224      %else %if 16_FC00 <= theta
  225        sin = -(16_10000-theta)*1608//1024
  226      %else
  227        print string("Oops, missed a case: sin "); phex(theta); newline; %stop
  228      %finish
  229    %finish
  230    
  231    %if xcos&16_FFFFC000 # 16_FFFFC000 %and xcos&16_FFFFC000 # 0 %then %start
  232      %if                  theta < 16_0400 %start
  233        cos = K16 - (theta)//25
  234      %else %if 16_3C00 <= theta < 16_4400
  235        cos = (16_4000-theta)*1608//1024
  236      %else %if 16_7C00 <= theta < 16_8000
  237        cos = -K16 + (16_8000-theta)//25
  238      %else %if 16_8000 <= theta < 16_8400
  239        cos = -K16 + (theta-16_8000)//25
  240      %else %if 16_BC00 <= theta < 16_C400
  241        cos = (theta-16_C000)*1608//1024
  242      %else %if 16_FC00 <= theta
  243        cos = K16 - (16_10000-theta)//25
  244      %else
  245        print string("Oops, missed a case: cos "); phex(theta); newline; %stop
  246      %finish
  247    %finish
  248  %end
?HALFBASE unused
  249  
  250  !%external %real %fn %spec sin %alias "sinf" (%real angle)  ;! use C's math lib to check results.
  251  !%external %real %fn %spec cos %alias "cosf" (%real angle)
  252  
  253  %real Theta
  254  %integer i, sinTheta, cosTheta, CheckSinTheta, CheckCosTheta, sinErr = 0, cosErr = 0, err
  255  
  256  %routine pround(%integer f, n)
  257    ! This is a quick (and rather ugly) hack due to the current Imp compiler
  258    ! not implementing all of the usual floating point I/O library calls yet
  259    
  260    %real r, sign = 1.0
  261    %integer tens = 0, p
  262  
  263    %if f=0 %start
  264      r = 0
  265    %else
  266      f = -f %and sign = -1.0 %if f < 0
  267      r = f / K16
  268      r = r * 10.0 %and tens = tens+1 %until 100 <= INT PT(r) < 1000
  269      p = tens
  270      r = INT(r)  { round to 3 significant digits }
  271      r = r / 10.0 %and tens = tens-1 %while tens > 0
  272      r = r + 0.000001 { handle real values not exactly representable in decimal }
  273    %finish
  274    spaces(n - (p+3))
  275    %if sign < 0 %then print symbol('-') %else print symbol(' ')
  276    print fl(r, p+2) { library call truncates, doesn't round :-( }
  277  %end
  278  
  279  %on %event * %start
  280    newline
  281    %if event_event = 14 %and event_sub{event} = 1 %start
  282      ! These are generated outside of the Imp77 environment, such as by Valgrind or GCC's Undefined Behaviour detection (ubsan).
  283      %if event_info{extra} = 4 %start
  284        ! SIGILL
  285        print string("Linux Signal SIGILL trapped at ")
  286        print string(event file)
  287        print string(" line")
  288        write(event line, 0)
  289        print string(" - this was probably raised by ubsan (gcc -fsanitize-trap=undefined)")
  290        newline
  291      %else %if event_info{extra} = 8
  292        ! SIGFPE
  293        print string("Linux Signal SIGFPE trapped at ")
  294        print string(event file)
  295        print string(" line")
  296        write(event line, 0)
  297        print string(" - this is a floating point error (such as divide by zero)")
  298        newline
  299      %else %if event_info{extra} = 8
  300        ! SIGSEGV
  301        print string("Linux Signal SIGSEGV trapped at ")
  302        print string(event file)
  303        print string(" line")
  304        write(event line, 0)
  305        print string(" - this may be from a NULL pointer or accessing past the end of an array")
  306        newline
  307      %else
  308        ! unknown
  309        print string("Linux Signal "); write(event_info{extra}, 0); print string(" trapped."); newlines(2)
  310      %finish
  311    %else
  312      print string("Imp77 %signal ")
  313      write(event_event,0); print string(", ")
  314      write(event_sub{event},0); print string(", ")
  315      write(event_info{extra},0)
  316      print string(" raised in ")
  317      print string(event file)
  318      print string(" line")
  319      write(event line, 0)
  320      newline
  321    %finish
  322    print string(event message)
  323    %stop
  324  %finish
  325  
  326    ! We get about 3 significant digits of accuracy
  327    print string("Note: this CORDIC was implemented to only 14 bits of accuracy for 16 bit machines,"); newline
  328    print string("      so we will report results to 3 significant digits."); newlines(2)
  329    
  330    SinCos(INT(-9*K64/(2*PI)), sinTheta, cosTheta)
  331    printstring("sin and cos of  -9 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
  332  
  333    SinCos(INT(0*K64/(2*PI)), sinTheta, cosTheta)
  334    printstring("sin and cos of   0 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
  335  
  336    SinCos(INT(1.5*K64/(2*PI)), sinTheta, cosTheta)
  337    printstring("sin and cos of 1.5 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
  338  
  339    SinCos(INT(6*K64/(2*PI)), sinTheta, cosTheta)
  340    printstring("sin and cos of   6 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
  341  
  342  
  343    %if debug # 0 %start
  344      %for i = 0, 4096, K64 %cycle  { A circle is 64K units }
  345        Theta = i*2*PI/K64 { A circle is 2Pi radians }
  346    
  347        SinCos(i, sinTheta, cosTheta)
  348    
  349        print string("Cordic:         Theta = "); phex(i)
  350        print string("; Sin(Theta) = "); write(sinTheta, 0)
  351        print string("; Cos(Theta) = "); write(cosTheta, 0)
  352        newline;
  353        
  354        CheckSinTheta = INT(sin(Theta)*K16); CheckCosTheta = INT(cos(Theta)*K16);
  355        
  356        print string("Floating point: Theta = "); phex(INT(Theta/(2*PI)*K64))
  357        print string("; Sin(Theta) = "); write(CheckSinTheta, 0)    
  358        print string("; Cos(Theta) = "); write(CheckCosTheta, 0)
  359        newlines(2)
  360    
  361        err = CheckSinTheta - sinTheta; err = -err %if err < 0; sinErr = sinErr + err
  362        err = CheckCosTheta - cosTheta; err = -err %if err < 0; cosErr = cosErr + err
  363    
  364      %repeat
  365    
  366      { Use this to test if algorithmic tweaks improve the accuracy }
  367      print string("Cumulative error:  sin = "); write(sinErr, 0)
  368      print string(",  cos = "); write(cosErr, 0)
  369      newline
  370      
  371    %finish
  372  %end %of %program
?K8 unused

  297 Statements compiled
