Cordic Algorithm for ATAN -- what to do with cases where x and/or y == 0?

Zoot · 2012-11-16 20:18

Hello all -- I've got a balance 'bot I'm working on and I'm using some ATAN code that Bean wrote for SX/B.

I'm not posting this in the SX forum, because my code works perfectly well and I don't have a technical question per se.

Rather, I am unsure about one operation in the Cordic routine -- where the x and y coordinates are normalized before running the cordic loop.

In Bean's code, he normalizes x and y to be 4096 <= val <= 8192.

But if that's true, then if x == 0 or y == 0, then this version of the Cordic algorithm goes into an infinite loop when trying to normalize the value to be >= 4096.

To solve the hang, I tried two versions -- one where I initialized x or y to 1 if they were 0, the other where I just skipped the minimum normalization altogether for zero value cases.

In practice, both gave me similar results when calculating rotation from a 3-axis accelerometer, but then I realized I would be better served finding out what should be done with such edge cases (or if this code version of Cordic is missing something?).

The full routine is below, but the relevant snippet is here:

' obviously, the loop below will hang if either xcoord == 0 or ycoord == 0
  DO
    IF yCoord > 4096 THEN EXIT
    IF xCoord > 4096 THEN EXIT
    yCoord = yCoord << 1
    xCoord = xCoord << 1
  LOOP

Full routine:

' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------

ATAN:
  ' Get parameters
  xCoord = __WPARAM12
  yCoord = __WPARAM34
 
  ' Initialize angle
  angle = 0

  ' Get sign of yCoord
  yNegative = yCoordSign

  ' If yCoord was negative then rotate coordinate
  IF yNegative = 1 THEN
    yCoord = -yCoord
    xCoord = -xCoord
  ENDIF

  ' Get sign of xCoord
  xNegative = xCoordSign

  ' If xCoord was negative then rotate coordinate
  IF xNegative = 1 THEN
    wTemp1 = yCoord
    yCoord = -xCoord  
    xCoord = wTemp1
  ENDIF

  ' Scale xCoord and yCoord
  DO
    IF yCoord < 8192 THEN 
      IF xCoord < 8192 THEN EXIT
    ENDIF
    yCoord = yCoord >> 1
    xCoord = xCoord >> 1
  LOOP

' obviously, the loop below will hang if either xcoord == 0 or ycoord == 0
  DO
    IF yCoord > 4096 THEN EXIT
    IF xCoord > 4096 THEN EXIT
    yCoord = yCoord << 1
    xCoord = xCoord << 1
  LOOP

  ' Main CORDIC loop
  FOR cnt=0 to 14

    ' Get Absolute value of yCoord
    wTemp1 = yCoord
    IF yCoordSign = 1 THEN
      wTemp1 = -wTemp1
    ENDIF

    ' Get Absolute value of xCoord
    wTemp2 = xCoord
    IF xCoordSign = 1 THEN
      wTemp2 = -wTemp2
    ENDIF

    ' Shift temp1 and temp2 right (cnt) times
    wTemp1 = wTemp1 >> cnt
    wTemp2 = wTemp2 >> cnt

    ' Remember sign of xCoord before it is adjusted
    xSign = xCoordSign

   ' Adjust xCoord
    xCoord = xCoord + wTemp1

   ' Get ATAN from table
    cnt = cnt << 1
    READ ATAN_Table + cnt, wTemp1
    cnt = cnt >> 1

    ' Adjust angle
    IF yCoordSign = 1 THEN
      angle = angle - wTemp1
    ELSE
      angle = angle + wTemp1
    ENDIF

    ' Adjust yCoord; If signs are equal then subtract, if signs are different then add
    IF yCoordSign = xSign THEN
      yCoord = yCoord - wTemp2
    ELSE
      yCoord = yCoord + wTemp2
    ENDIF

  NEXT
  ' CORDIC loop done

  ' Adjust angle
  IF yNegative = 1 THEN
    angle = angle + 32768
  ENDIF

  IF xNegative = 1 THEN
    angle = angle + 16384
  ENDIF


  RETURN angle

' -------------------------------------------------------------------------
' Data Tables
' -------------------------------------------------------------------------

ATAN_Table: ' 15 Word entries; Atan(1/(2^position)) in brads * 256
  WDATA 8192, 4836, 2555, 1297, 651, 326, 163, 81, 41, 20, 10, 5, 3, 1, 1

Tracy Allen · 2012-11-16 21:30

Think about the meaning of ATAN. The angle theta that has a tangent of y/x. If case
y=0, x>0 --> theta=0°
y=0, x<0 --> theta = -180° (but in brads)
y>0, x=0 --> theta = +90°
y<0, x=0 --> theta = -90°
y=0, x=0 --> theta undefined (no vector for a point at the origen)
Dispose of those cases at the top of the algorithm, before attempting to scale.

Zoot · 2012-11-16 22:22

Yes, of course. Thanks Tracy.

Cordic Algorithm for ATAN -- what to do with cases where x and/or y == 0?

Comments