Search code examples
pythoncompassmagnetometer

Magnetometer Heading Calculation?

I am using MPU9250 from adafruit (https://learn.adafruit.com/adafruit-tdk-invensense-icm-20948-9-dof-imu)

This sensor is outputting raw data of magnetometer.

I was able to calibrate data from

enter image description here

to

enter image description here

where I get calibration number by

x offset = (max_x+min_x)/2
y offset = (max_y+min_y)/2
z offset = (max_z+min_z)/2

and I subtract offset to raw data.

However, I used calculation from (https://cdn-shop.adafruit.com/datasheets/AN203_Compass_Heading_Using_Magnetometers.pdf)

Direction (y>0) = 90 - [arcTAN(x/y)]*180/PI
Direction (y<0) = 270 - [arcTAN(x/y)]*180/PI
Direction (y=0, x<0) = 180.0
Direction (y=0, x>0) = 0.0

However, direction is alway off and incorrect.

for example, when I amd heaind 0 degree (directly to the north)

x  15.749999999999998
y  -0.9749999999999996
z  47.4

and caluation gives 40.44 degree

When I am heading 90 degree (East)

x  4.049999999999999
y  -14.475
z  46.949999999999996

heading is 84.906

When I am heading 270 degree (West)

x  1.3499999999999996
y  7.125
z  45.0

Heading is 75.774 Degree

When I am heading 180 degree (to South)

x  -11.55
y  -2.9250000000000007
z  46.5

and calucation is 128.99 degree

Does my methodology to calcuate heading is wrong? or my calibration needs more step?

[Edit]

Magnetometer Calibration:

def calibrationDataCollection():
    i2c = board.I2C()  # uses board.SCL and board.SDA
    icm = adafruit_icm20x.ICM20948(i2c)
    
    lx = kbhit.lxTerm()
    lx.start()
    x=[]
    y=[]
    z=[]
    while True:
        #print(icm.magnetic)

        if lx.kbhit(): 
            c = lx.getch()
            c_ord = ord(c)
            if c_ord == 32: # Spacebar
                print("\nStop")
                break
        x.append(icm.magnetic[0])
        y.append(icm.magnetic[1])
        z.append(icm.magnetic[2])

        print((max(x)+min(x))/2)
        print((max(y)+min(y))/2)
        print((max(z)+min(z))/2)
        print()
        time.sleep(0.1)
    
    comp_df = pd.DataFrame({"x": x, "y": y, "z": z})
    comp_df["offset_x"] = (comp_df.x.max()+comp_df.x.min())/2
    comp_df["offset_y"] = (comp_df.y.max()+comp_df.y.min())/2
    comp_df["offset_z"] = (comp_df.z.max()+comp_df.z.min())/2
    toCSV(analysisPath, "Calibration_2.csv", comp_df)

I calibrate the sensor by moving 8 shape in 3D (like infinite loop shape)

Then, Whenever offset values become steady, I stop calibration and convert it to csv file.

Then, use offset value, I am getting calibrated value

def Compass_2():
    i2c = board.I2C()  # uses board.SCL and board.SDA
    icm = adafruit_icm20x.ICM20948(i2c)


    while True:
        x = icm.magnetic[0]- 12.15 #(these are my calibration offsets)
        y = -icm.magnetic[1]+ 6.225
        z = icm.magnetic[2]+ 14.4
        print("x ", x)
        print("y ", y)
        print("z ", z)

        heading = magToHeading2([x,y,z])
        
        print("heading: ", heading)
        time.sleep(0.5)

The heading(magToHeading2) is calucated by

def magToHeading2(magnetic):
    heading = -1

    if magnetic[1] > 0:
        heading = 90-math.atan(magnetic[0]/magnetic[1]) * 180 / math.pi
    elif magnetic[1] < 0:
        heading = 270-math.atan(magnetic[0]/magnetic[1]) * 180 / math.pi
    elif magnetic[1] == 0:
        if magnetic[0] < 0:
            heading = 180.0
        else:
            heading = 0.0
    
    return round(heading,3)
Solution

  • The problem was calibration. Eventhough after calibration data are all well centered, whenever I rotate the sensor with horizonal surface, the data were not well cenetered. So, instead of shaking around the sensor for calibraiton, I just put sensor on the table and rotate around (since I am using only xy for heading).