Calculating a compass heading using a 5883 magnetometer

[dr pepper]

I'd like to use a 5883 magnetometer to calculate a compass heading in degrees using the X and Y values.

The math I was thinking of is:

arctan (x/y) , where x and y are the magnetometer readings.

This gives me an angle within 90 degrees, to work out which quadrant I'm in I was thinking of just looking at the + and - sign of the x and y readings, and adding 90,180 or 270 degrees.

Piclist have a handy routine I can cut/paste to do the arctan of a fraction.

Does this sound like an effective approach?

 

[esp1]

hi dr,
When converting GPS Data Lat and Long coordinates into Azimuth angle/degrees, the Arctan method is used.
Also as you say, the SIGN of the Lat/Long, X,Y, in your case can be used to find and adjust for the Quadrant.

E
This is the Oshonsoft Basic I use, has 6 decimal place accuracy.
'quad' is calculated from the SIGNs

'calc ratio of diffs
atn = londiff1 / latdiff1

'calc the angle in degrees using ratio
atn2 = atn * atn
If atn <= 1 Then
Arctan = atn / (0.281125 * atn2 + 1) * 57.296
Else
Arctan = (1.5708 - atn / (atn2 + 0.28125)) * 57.296
Endif

'convert angle to azimuth 0 thru 360 deg
Select Case quad
Case 0
Arctan = Arctan
Case 1
Arctan = 180 - Arctan
Case 2
Arctan = 180 + Arctan
Case 3
Arctan = 360 - Arctan
EndSelect

 

[dr pepper]

My knowledge of C is enough to understand that.
Why arctan squared?
I have to do this in assembler, going to be a headache.

 

[esp1]

hi dr,
I squared the Atn prior to the main part of the routine, in Oshonsoft, it was faster to calc in that way, possibly due the compiler operation.
Its not essential.
I would be interested to see your asm version.
E
BTW: I am using a 18F4520 20MHz Xtal

 

[dr pepper]

This is the code I found, it is not my work.
I was hoping just to scale the x and y 16bit output of the magnetometer to 8 bits shove it into this then multiply the output of this by 180/pi to convert fropm radians to degrees.

       list    p=16C84,t=ON,c=132,n=80,st=off


        include "P16C84.INC"

  cblock   0x0C
    temp
    x,y,a
    x1,y1,cnt
    result
  endc


        ORG     0               ;Reset Vector

        GOTO    Main

        ORG     4               ;Interrupt Vector


Main


        CLRF    x
	clrf	x1
	movlw	0x30
	movwf	y1
	movwf	x1
l1
	movf	y1,w
	movwf	y

	movlw	3
	addwf   x1,w
	movwf	x1
	movwf	x


        CALL    FRAC_DIV

	movf	a,w
	movwf	x
	CALL	arctan

        goto    l1

FRAC_DIV:
;-------------------
;Fractional division
;
; Given x,y this routine finds:
;  a = 256 * y / x
;

    movlw  8    ;number of bits in the result
    movwf  cnt
    clrf   a    ; the result
    movf   x,w

L1:

    clrc
    rlf    y,f   ;if msb of y is set we know x<y
    rlf    a,f   ;and that the lsb of 'a' should be set
    subwf  y,f   ;But we still need to subtract the
                 ;divisor from the dividend just in
                 ;case y is less than 256.
    skpnc        ;If y>x, but y<256
     bsf   a,0   ; we still need to set a:0


    btfss  a,0   ;If y<x then we shouldn't have
     addwf y,f   ;done the subtraction

    decfsz cnt,f
     goto  L1

    return


;----------------------------------------------------------
;
;arctan (as adapted from the similar arcsin function)
;
;  The purpose of this routine is to take the arctan of an
;8-bit number that ranges from 0 < x < 255/256. In other
;words, the input, x, is an unsigned fraction whose implicit
;divisor is 256.
;  The output is in a conveniently awkward format of binary
;radians (brads?). The output corresponds to the range of zero
;to pi/4 for the normal arctan function. Specifically, this
;algorithm computes:
;
; arctan(x) = real_arctan(x/256) * 256 / (pi/4)
;  for 0 <= x <= 255
;  
;  where, real_arctan returns the real arctan of its argument
;in radians.
;
;  The algorithm is a table look-up algorithm plus first order
;linear interpolation. The psuedo code is:
;
;unsigned char arctan(unsigned char x)
;{
;  unsigned char i;
;
;  i = x >> 4;
;  return(arctan[i] + ((arctan[i+1] - arctan[i]) * (x & 0xf))/16);
;}
;
;


arctan

        SWAPF   x,W
        ANDLW   0xf
        ADDLW   1
        MOVWF   temp                    ;Temporarily store the index
        CALL    arc_tan_table           ;Get a2=atan( (x>>4) + 1)
        MOVWF   result                  ;Store temporarily in result

        DECF    temp,W                  ;Get the saved index
        CALL    arc_tan_table           ;Get a1=atan( (x>>4) )

        SUBWF   result,W                ;W=a2-a1, This is always positive.
        SUBWF   result,F                ;a1 = a1 - (a1-W) = W

        CLRF    temp                    ;Clear the product
        CLRC

        BTFSC   x,0
         ADDWF  temp,F
        RRF     temp,F
        CLRC

        BTFSC   x,1
         ADDWF  temp,F
        RRF     temp,F
        CLRC

        BTFSC   x,2
         ADDWF  temp,F
        RRF     temp,F
        CLRC

        BTFSC   x,3
         ADDWF  temp,F
        RRF     temp,W

        ADDWF   result,F

        RETURN

arc_tan_table
        ADDWF   PCL,F
        RETLW   0
        RETLW   20     ;atan(1/16) = 3.576deg * 256/45
        RETLW   41
        RETLW   60
        RETLW   80
        RETLW   99
        RETLW   117
        RETLW   134
        RETLW   151
        RETLW   167
        RETLW   182
        RETLW   196
        RETLW   210
        RETLW   222
        RETLW   234
        RETLW   245
        RETLW   0	;atan(32/32) = 45deg * 256/45

        END

 

[Venkadesh_M]

What controller is this?

In simple controllers the better way is LUT only.

 

[esp1]

hi dr,
Your code assembles OK, but I need to know the format of the data from the 5883.
Could you post a few data strings from the 5883.?

Also what resolution do you require for the azimuth angle in degrees.?
The Table in your code only returns a 5 Deg interval
E

 

[dr pepper]

I'd like 1 degree accuracy.

The magnetometer has a 16 bit output per axis, there are 3 axes.

This is a snip from realterm, my code reads the registers via 12c then converts it into ascii and shows negative values with a -, the first and third values are x and y, the last 3 digit value is the output of the maths routine I posted, to keep it simple I'm inputting the lower byte into x and y.

I probably should divide the 16 bit output from the magnetometer, its max value is around 1200.

00242 -00944 -00300 015

00254 -00886 -00663 015

00254 -00875 -00697 015

00250 -00864 -00713 015

00253 -00861 -00726 015

00249 -00854 -00734 015

00247 -00852 -00743 015

00245 -00847 -00748 015

00248 -00846 -00753 015

00243 -00840 -00757 008

00245 -00842 -00762 006

 

[esp1]

I guess you realise the Table in that code is only for 45 degrees.? with a very rough approximation to Atn.
So thats 8 sectors for 360deg bearings.

ARC_TAN_TABLE
        ADDWF   PCL,F
        RETLW   0 ;0 deg
        RETLW   0x03           ;3 deg          ;ATAN(1/16) = 3.576DEG * 256/45
        RETLW   0x29
        RETLW   0x3C
        RETLW   0x0e;;;;50          ;14.0 deg
        RETLW   0x63
        RETLW   0x75
        RETLW   0x86
        RETLW   0x1a;;;;97            ;26.5 deg
        RETLW   0xA7
        RETLW   0xB6
        RETLW   0xC4
        RETLW   0x24;;;;;D2             ;36.8 deg
        RETLW   0xDE
        RETLW   0xEA
        RETLW   0xF5
        RETLW   0x2d;;;0x FF           41 deg       ATAN(32/32) = 45DEG * 256/45

        END

I have had to change the numeric values to hex, my assembler complained.
The changed values prefixed by ;;;; are some tests I did with degrees.

Have you considered a Table of say 90 entries, 0 thru 90degrees.?

EDIT:
Looking thru your data columns 1 and 3 for X and Y, have you manually determined the bearing? if yes can you post.

 

[dr pepper]

The last 3 digits was my attempt to derive the heading in radians.

I am out of my depth with the maths, I think I'd better find another approach.
Learning C is probably a good idea.

- - - Updated - - -

Well I had one last try, and I did make some silly mistakes.

Now as I rotate the magnetometer the x and y values generate a value in radians that goes from 0 to 15 (4 times for 360 rotation), implying that there is a 3 degree resolution.
There is a little snafu though, every so often the radian valus goes really high, but I wonder if thats one of the x or y variables going to 0.

I will play some more tomoz, detecting the octant shouldnt be so hard compared to this, then it sjust some straightforward maths to get my 0 - 360, 3 degrees resolution will have to do.

 

[esp1]

hi dr,
Which PIC are planning to use on the project.?

E

 

[dr pepper]

I have been using a 16f628, I'd like to stick to the 16 series.