Magnetometer compass calculations

[dr pepper]

I want to calculate a compass heading from a 5883 magnetometer sensor, using x and y readings.

To get the heading angle in degrees within a quadrant of 90 degrees I was thinking of:

arctan (x/y) (x and y being the magnetometers readings).

This gives me an angle within a 90 degree quadrant of 360 degrees, from this I can work out which quadrant the heading is in by looking at the + and - of the x and y values. and doing some simple maths subtracting 90,180 or 270 degrees to get the heading.

Does this seem like a sensible approach.

 

[steveB]

if you are programming in a language that supports the arctan2 function, use that one. It automatically checks the +/- signs to give a full 360 degree range and identify the correct heading.

 

[dr pepper]

I was wondering what the squared was for.
Unfortunatly I dont I use assembler.
But I did find some usefull stuff on roman blacks website, he does it using really simple maths and correcting the result, the only thing is I'll have to convert his C code to asm.

 

[Diver300]

I think that your approach is a good one, but there is one additional feature that I would suggest.

I wrote some assembly code to work out angle from x and y values. It works in the following way.

1) Work out and store which quadrant the angle is from the signs of the x and y values.
2) Work out and store if the y value is bigger than the x value. If it is, the angle range is 0 - 45, 135 - 225 or 315 - 360
3) Calculate the smaller of |x/y| and |y/x|, and work out the arctan of that. The result is 0 - 45 degrees.
4) If the angle range (from steps 1 and 2) is 45 - 90, 135 - 180, 225 - 270 or 315 - 360, take the negative of the arctan that you just calculated
5) Add 0, 90, 180, 270 or 360 to the result

The reason for taking the smaller of |x/y| and |y/x| is so that you don't have to calculate the arctan of a huge number near 0 or 360 degrees. I used a 5 position look-up table, with linear interpolation, for the arctan table, to give an error or less than 1 degree for the range 0 - 45 degrees, and this approach meant that I never needed a wider range, and I was able to avoid handling large numbers.

The table lookup and linear interpolation idea came from

I also used a lookup that took in 3 binary values, the signs of x and y, and whether x was larger than y. The output of this lookup was the axis angle, (0, 90, 180, 270 or 360) and whether the 0 - 45 needed to be added or subtracted.

 

[dr pepper]

I found something similar on piclist, it only does 45 degree though, but it does arctan.
Its been a battle to try and get it to work.

 

[Diver300]

The advantage of keeping the angle under 45 degrees is that the number that you are trying to find the arctan of is in the range 0 - 1, and arctan is fairly linear, so you never need to handle big numbers, and the linear interpolation works very well.

You are just using the fact that arctan(f) = 90° - arctan(1/f). In this case f = |x/y|, so if x>y, you calculate f1 = 1/f = |y/x|.

In that case, f is never calculated. f1, which is |y/x| is calculated, and is less than 1, and its arctan is calculated instead. Then it is taken away from 90 ° to find the arctan(f)

What processor are you using. I have code examples for 8 and 16 bit processors if that would help.

 

[dr pepper]

I'm using a 16f628a for this project.
Code examples would be great, maths isnt my scene.

 

[Diver300]

Here is the code that I have been using. It was actually written for an 18 series PIC, so I've had to make some changes as there are differences in the instructions and the lookups work differently.

You need to make sure that the lookup doesn't straddle a page boundary.

Other than that, you just need to define the registers.
x_input and y_input are in 2s compliment, and the result is degrees in count2:count1
I think that it is accurate to the nearest degree.
0° is straight up, where x = 0 and y is positive. 90° is where x is positive and y = 0. If you want different axes, you can probably just change the lookup table.

As I said in the previous texts, it manages this with 8 bit maths by never trying to work out big numbers. So if x = 20 and y = 100, it will work out x/y and take the arctan of that. If x = 100 and y = 20, it will work out y/x, and take the arctan of that. The arctangents are always 0 - 45° and they are added to or subtracted from 0, 90, 180, 270 or 360°

The code can take quite some time to run as there is a divide which is just done by subtracting lots of times, and it can take around 256 iterations.

I hope this code helps.

;we now have 8 ranges of 45 degrees each to sort out
;x_input and y_input are in 2s compliment

    clrf    angle_range
    btfss     y_input, 7                    ;see if y is negative
    goto     north_half
    bsf        angle_range, 0                ;this means that the bearing is in the south half
    comf    y_input, f   
    incf    y_input, f                    ;negate if needed so that y is now positive
north_half   
    btfss    x_input, 7
    goto    quadrants_done
    bsf        angle_range, 1                ;this mans that the bearing is in the west half
    comf    x_input, f                   
    incf    x_input, f                    ;negate if needed so that x is now positive
quadrants_done

    clrf    bottom_byte
    movlw    0xff
    movwf    count       

    movf    y_input, w
    subwf    x_input, w

    btfsc    STATUS, C
    goto    no_xy_swap    ;compare longditude difference with latitude difference

    bsf        angle_range, 2            ;note if needed and swap the registers
                                    ;this means that the y was larger, which happens if the
                                    ;bearing is nearer to North or South than it is to East or West

    movf    y_input, w
    subwf    x_input, w
    addwf    y_input, f
    subwf    x_input, f        ;swap x and y differences

no_xy_swap
;x is now largest

;This divides the y * 256 by the x, optimised for code length
divide_loop
    incf    count, f    ;count the number of subtractions

    incf    count, w
    btfsc    STATUS, Z
    goto    divide_done    ;skip out if the divide is taking too long

    movf    x_input, w
    subwf    bottom_byte, f

    btfss    STATUS, C
    decf    y_input, f    ;subtract 1 bytes from 2 bytes

    btfss    y_input, 7
    goto    divide_loop            ;this keeps the routine returning to here until divide is finished

divide_done

;count is now 0 - 0xff representing tan(angle) in the range 0 - 45 deg
;there is a lookup table for the values, 0x00, 0x40, 0x80, 0xC0 and 0x100
;the value looked up is 4 times the tan in degrees
;the values either side of the current tan are looked up
;the last 6 bits are multiplied by the difference in the lookup table
;we want the atan of the number /256

    clrf    count1
    bcf        STATUS, C
    rlf        count, f
    rlf        count1, f
    rlf        count, f            ;collect the top 2 bits and
    rlf        count1, f            ;multiply what is left by 4, because it will later be divided by 4
    incf    count1, f   

    call    arc_tan_table        ;Get a2=atan( (x>>6) + 1)
    movwf    count2                ;Store temporarily in count2
    decf    count1, f            ;Get the saved index
    call    arc_tan_table        ;Get a1=atan( (x>>6) )
    subwf    count2, w            ;W=a2-a1, This is always positive.
    subwf    count2, f            ;a1 = a1 - (a1-W) = W    ;now count2 is the lower value and w is the difference

;w needs to be multiplied by count
    clrf    multh
;    clrf    multl                ;not needed but can be left in to make the multiply easier to understand
    clrf    mult_count
    bsf        mult_count, 3        ;set to 8 without using w

mult_loop
    bcf        STATUS, C
    btfsc    count, 0
    addwf    multh, f            ;add if needed
   
    rrf        multh, f            ;rotate result
;    rrf        multl, f            ;not needed but can be left in to make the multiply easier to understand
    rrf        count, f            ;rotate count

    decfsz    mult_count, f
    goto     mult_loop            ;the difference is multiplied,
    movf    multh, w            ;take the high byte

    addwf    count2, f
    bcf        STATUS, C
    rrf        count2, f           
    bcf        STATUS, C
    rrf        count2, w   
    movwf    count                ;divide by 4   

;now count is the angle in degrees from 0 - 45
;angle_range says which quadrant and which half the angle is in

;0    45 - 90            0x82
;1    90 - 135        0x02
;2    270 - 315        0x04
;3    225 - 270        0x84
;4    0 - 45            0x01
;5    135 - 180        0x83
;6    315 - 360        0x85
;7    180 - 225         0x03


;The lookup table ends with the last 3 bits representing how many lots of 90 degrees the start angle is, +1
;and bit 7 is set if the value in count should be taken as negative

    call    angle_range_table
    movwf    angle_range


    btfss    angle_range, 7
    goto    angle_increment_positive
    comf    count, f   
    incf    count, f        ;negate if required
angle_increment_positive

    movlw    d'256'-d'45'
    movwf    count1                ;start value set so that after one addition the result is zero
    bcf        angle_range, 7
    movlw    d'45'
next45
    addwf    count1, f   
    decfsz    angle_range, f
    goto     next45                    ;add 45 the correct number of times

    bcf        STATUS, C
    clrf      count2
    rlf        count1, f            ;multiply by 2
    rlf        count2, f            ;now 0, 90, 180, 270 or 360
    movf    count, w
    addwf    count1, f
    btfsc    STATUS, C
    incf    count2, f
    btfsc    count, 7
    decf    count2, f                ;add the difference, dealing with carry and negatives as required

    btfss    count2, 0
    goto    no_roll
    movlw    d'360'-d'256'
    subwf    count1, w

    btfss    STATUS, C
    goto    no_roll
    movwf    count1
    clrf    count2                    ;mod 360 in case nothing is subtracted from 360
no_roll

;and that's it. The angle in degrees is in count2:count1



arc_tan_table
    btfsc    count1, 2
    retlw    d'182'

    btfsc    count1, 1
    goto    top_half_atan_table

    btfss    count1, 0
    retlw    d'3'
    retlw    d'59'

top_half_atan_table
    btfss    count1, 0
    retlw    d'110'
    retlw    d'150'


angle_range_table
    movlw     high(angle_range_lookup_start)
    movwf    PCLATH

angle_range_lookup
    movf    angle_range, w
    andlw    b'00000111'
    addwf    PCL, f
angle_range_lookup_start
    retlw            0x82
    retlw            0x02
    retlw            0x04
    retlw            0x84
    retlw            0x01
    retlw            0x83
    retlw            0x85
    retlw            0x03

 

[dr pepper]

Totally fantastic.
I'm at work right now so I have printed that off, something to read at 2am brew.
Sounds like all I need to do is reduce the xy to 8 bit values, this will take some bit fiddling as they are 16 bit values but only go to around 500, so to keep resolution I need to delete some bits between the sign bit and the actual value, and the answer is in count 1 and count 2, excellent.
Favour owed.
I'll let you know how I get on, and maybe post some pics, one thing I want to do is to put a magnetometer in a vintage aircraft radio compass guage so it'll work as a compass.

heres the clock: https://www.electro-tech-online.com...mentation-stepper-motors.142385/#post-1196115

 

[Diver300]

In 2s compliment, numbers of small magnitude have several of the most significant bits that are the same as the sign bit. On the 5983 sensor, it has an output range of 0xF800–0x07FF (-2048–2047 ) so the first 5 bits area always the same, and it doesn't matter which you use for the sign bit. You can just shift the number by 4 bits to give you an 8 bit number that keeps the correct sign.

    rlf        x_input_low, f
    rlf        x_input, f
    rlf        x_input_low, f
    rlf        x_input, f
    rlf        x_input_low, f
    rlf        x_input, f
    rlf        x_input_low, f
    rlf        x_input, f

If you start with the 16 bit value in the range 0xF800–0x07FF in x_input:x_input_low, then run that code, you end up with an 8 bit value in x_input that is the range -128–127 that is the original value divided by 16. The sign is preserved.

 

[dr pepper]

Right, cut and pasted and glued that to my code.
It works.
I changed your shift routine to right shift, and used the low byte, seemed to work for me and I understood that better.
I pulled my hair out for quite a while with strange headings, there were 2 reasons for this, one stray magnetic fields from me and the surroundings, and the other is that the hmc5883 has its polarity so that you have to hold the text on the chip down towards the earth to get the readings to go the right way, ie to get the heading to go up in degrees as you turn the magnetometer clockwise.

 

[Diver300]

I'm glad you got it to work.

I did consider using a right shift. You could also have a routine that uses swaps that might save a line or two. Whatever you do, it is taking the middle 8 bits from a 16 bit number, made from two 8-bit numbers, and there are lots of ways of doing that.

You might be able to change the lookup table to get the rotation that you want. The input to the lookup table is 0 - 7, with bits 0, 1 and 2 representing whether x is negative, whether y is negative, and whether the magnitude of y is larger than that of x. The output of the table is the number of times 90 ° has to be added, and whether that 0 - 45 ° should be added or subtracted. I think that you could rearrange the table so that the rotation was in the other direction, and started at a different point. The rest of the code should be the same. That bit is never obvious to me. It takes a bit of paper and some trial and error to get it right.

 

[dr pepper]

Yes I know what you mean, to sort it out all I need to do is unsolder a 4 pin header and put it through the other side of the board, which I have allready done, so I'm ok, however I'll be using this again at some point and that seems like a simple thing to do.

I'm surprised that my hand makes quite a bit of difference to the readings, more when I move, maybe I'm magnetic.

I want to put this on a boat and maybe in a car, not sure where would be a good place for a magnetometer, somewhere away from electrical cables and chunks of iron, and fed with a screened cable, chunks of iron being hard to avoid in a car, maybe also take a load of measurements and average them.

 

[Diver300]

I would guess that you should go for one or other bumper, the mirrors, or centrally on the roof. You may need to compensate like ship's binnacles do.

I suggest that you fit a readout, and try each position. For each position, turn the car all the way round and take readings every 45 degrees. An empty car park would be a good place for stuff like that. The lines between the car park spaces give you every 90 degrees. The intermediates you can guess. Google earth tells you the angle of the car park marks.

Take a simple magnetic compass as well, to check that there aren't hidden water mains or something giving big local magnetic deviations.

It may be a good idea to record x, y, and z readings. The inclination of the magnetic field is around 68 ° in northern england, so the vertical field will be more than twice as strong as the horizontal field, and objects above and below the sensor may affect the field a lot.

It could be you find a good enough position that you can use the angle. If not, you may need a lookup table for corrections. Alternatively, you might find that more than one sensor is needed.

 

[dr pepper]

Ok thanks.
I am familiar with swinging the compass on a boat, incidently the readout part is often called the pelorus, though strictly speaking this is incorrect as a pelorus is a sighting 'scope, just another one of those name things.