kriswiner / MPU9250

Arduino sketches for MPU9250 9DoF with AHRS sensor fusion
1.03k stars 471 forks source link

calibration routine returns mag biases of zero #71

Open sbishop61 opened 8 years ago

sbishop61 commented 8 years ago

Hi, I added the calibration routine magcalMPU9250(float * dest1, float * dest2) .

I am using a pro mini and a nrf51822 using the Redbear arduino set up. When I run it seems to work generally but the calibration routine returns mag biases of zero which feels unlikely.

The explicit mag biases in the code used after the calibration are those in your code. I.e. I haven't changed them - I was trying to get them from the calibration.

Any ideas on the calibration?

MPU9250 I AM 71 I should be 71 MPU9250 is online... x-axis self test: acceleration trim within : 1.2% of factory value y-axis self test: acceleration trim within : 0.3% of factory value z-axis self test: acceleration trim within : -0.6% of factory value x-axis self test: gyration trim within : -2.3% of factory value y-axis self test: gyration trim within : -1.2% of factory value z-axis self test: gyration trim within : -0.5% of factory value MPU9250 initialized for active data mode.... AK8963 I AM 48 I should be 48 AK8963 initialized for active data mode.... Mag Calibration: Wave device in a figure eight until done! mag x min/max: 237 -294 mag y min/max: 228 -248 mag z min/max: 262 -268 Mag Calibration done! AK8963 mag biases (mG) 0.00 0.00 0.00 AK8963 mag scale (mG) 0.97 1.08 0.97 X-Axis sensitivity adjustment value 1.19 Y-Axis sensitivity adjustment value 1.19 Z-Axis sensitivity adjustment value 1.14 ax = -278.26 ay = -391.11 az = -167.30 mg gx = 93.42 gy = 67.23 gz = 201.93 deg/s mx = -915 my = 139 mz = -35 mG q0 = 0.00 qx = -0.36 qy = 0.40 qz = 0.84 Yaw, Pitch, Roll: -173.32, 37.49, 57.54 rate = 0.04 Hz ax = -163.39 ay = -127.50 az = -589.29 mg gx = -21.18 gy = 1.79 gz = -60.84 deg/s mx = -963 my = -32 mz = -305 mG q0 = 0.09 qx = -0.72 qy = 0.67 qz = 0.14 Yaw, Pitch, Roll: -100.77, 19.01, 176.55 rate = 176.94 Hz

kriswiner commented 8 years ago

Are you reading the mag scale factors from the Fuse ROM and applying these to get the mag offset biases? It is clear your offset biases are not zero, i.e., (237- 294)/2 = -27 which is not zero. But this needs to be scaled with the mag axis sensitivity read from the AK8963C FUSE ROM and then scaled by the conversion factor to get offset bias in corrected mGauss. I suspect your FUSE ROM values are zero since you are not reading them?

-----Original Message----- From: sbishop61 [mailto:notifications@github.com] Sent: August 22, 2016 5:00 PM To: kriswiner/MPU-9250 Subject: [kriswiner/MPU-9250] calibration routine returns mag biases of zero (#71)

Hi, I added the calibration routine magcalMPU9250(float * dest1, float * dest2) .

I am using a pro mini and a nrf51822 using the Redbear arduino set up. When I run it seems to work generally but the calibration routine returns mag biases of zero which feels unlikely.

The explicit mag biases in the code used after the calibration are those in your code. I.e. I haven't changed them - I was trying to get them from the calibration.

Any ideas on the calibration?

MPU9250 I AM 71 I should be 71
MPU9250 is online...
x-axis self test: acceleration trim within : 1.2% of factory value
y-axis self test: acceleration trim within : 0.3% of factory value
z-axis self test: acceleration trim within : -0.6% of factory value
x-axis self test: gyration trim within : -2.3% of factory value
y-axis self test: gyration trim within : -1.2% of factory value
z-axis self test: gyration trim within : -0.5% of factory value
MPU9250 initialized for active data mode....
AK8963 I AM 48 I should be 48
AK8963 initialized for active data mode....
Mag Calibration: Wave device in a figure eight until done!
mag x min/max:
237
-294
mag y min/max:
228
-248
mag z min/max:
262
-268
Mag Calibration done!
AK8963 mag biases (mG)
0.00
0.00
0.00
AK8963 mag scale (mG)
0.97
1.08
0.97
X-Axis sensitivity adjustment value 1.19
Y-Axis sensitivity adjustment value 1.19
Z-Axis sensitivity adjustment value 1.14
ax = -278.26 ay = -391.11 az = -167.30 mg
gx = 93.42 gy = 67.23 gz = 201.93 deg/s
mx = -915 my = 139 mz = -35 mG
q0 = 0.00 qx = -0.36 qy = 0.40 qz = 0.84
Yaw, Pitch, Roll: -173.32, 37.49, 57.54
rate = 0.04 Hz
ax = -163.39 ay = -127.50 az = -589.29 mg
gx = -21.18 gy = 1.79 gz = -60.84 deg/s
mx = -963 my = -32 mz = -305 mG
q0 = 0.09 qx = -0.72 qy = 0.67 qz = 0.14
Yaw, Pitch, Roll: -100.77, 19.01, 176.55
rate = 176.94 Hz

You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/71 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qkFtqsy9IXxo-4YCcJ3L_ D00PSJ9ks5qijf8gaJpZM4JqbKo . https://github.com/notifications/beacon/AGY1qonbzqvJkmk_l6bimdWyZFmc0ACzks5 qijf8gaJpZM4JqbKo.gif

sbishop61 commented 8 years ago

Kris, I copied the routine magcalMPU9250(float * dest1, float * dest2) from your sketch MPU9250_MS5637_AHRS_t3.ino

Aren't the following lines the FUSE ROM values?

X-Axis sensitivity adjustment value 1.19 Y-Axis sensitivity adjustment value 1.19 Z-Axis sensitivity adjustment value 1.14

I think I have found my error. I had copied between files and there was no call to getMres() before the calibration so mRes was not initialised. I have put a call in before the calibration and now get (I haven't plugged these values into the setting of magbias yet)

MPU9250 I AM 71 I should be 71 MPU9250 is online... x-axis self test: acceleration trim within : 1.2% of factory value y-axis self test: acceleration trim within : 0.3% of factory value z-axis self test: acceleration trim within : -0.3% of factory value x-axis self test: gyration trim within : -2.1% of factory value y-axis self test: gyration trim within : -1.5% of factory value z-axis self test: gyration trim within : 0.1% of factory value MPU9250 initialized for active data mode.... AK8963 I AM 48 I should be 48 AK8963 initialized for active data mode.... Mag Calibration: Wave device in a figure eight until done! mag x min/max: 220 -268 mag y min/max: 170 -231 mag z min/max: 232 -246 Mag Calibration done! AK8963 mag biases (mG) -42.73 -53.42 -12.01 AK8963 mag scale (mG) 0.93 1.14 0.95 X-Axis sensitivity adjustment value 1.19 Y-Axis sensitivity adjustment value 1.19 Z-Axis sensitivity adjustment value 1.14 ax = -269.84 ay = -165.16 az = 24.60 mg gx = 30.74 gy = 29.62 gz = 29.17 deg/s mx = -950 my = -23 mz = -76 mG q0 = 0.01 qx = -0.28 qy = 0.59 qz = 0.76 Yaw, Pitch, Roll: -172.82, 25.37, 81.04 rate = 0.04 Hz ax = 221.01 ay = -47.06 az = -855.16 mg gx = 57.56 gy = -40.51 gz = 68.65 deg/s mx = -883 my = -226 mz = -416 mG q0 = -0.04 qx = -0.55 qy = 0.82 qz = -0.14 Yaw, Pitch, Roll: -127.54, -13.23, -168.65 rate = 177.38 Hz

I am confused by the values once it has settled down as the accelerometer seems to be reading 2G, not 1G with significant accelerations in the horizontal plane.

Here y is the vertical axis, so shouldn't I expect ax=0 ay=-1000 az=0? (in mg)

ax = -157.65 ay = -2000.00 az = -688.23 mg gx = 0.51 gy = -0.08 gz = 0.16 deg/s mx = -112 my = 59 mz = -243 mG q0 = 0.46 qx = -0.69 qy = -0.44 qz = 0.35 Yaw, Pitch, Roll: 54.56, 4.53, -109.12 rate = 176.57 Hz ax = -158.69 ay = -2000.00 az = -688.96 mg gx = -0.34 gy = -0.16 gz = 0.08 deg/s mx = -112 my = 74 mz = -222 mG q0 = 0.47 qx = -0.69 qy = -0.43 qz = 0.34 Yaw, Pitch, Roll: 53.46, 3.53, -108.67 rate = 177.16 Hz

kriswiner commented 8 years ago

Yes, there is something wrong with your accel data; I would look at the raw values, likely you have an integer cast problem.

-----Original Message----- From: sbishop61 [mailto:notifications@github.com] Sent: August 22, 2016 6:28 PM To: kriswiner/MPU-9250 Cc: Kris Winer; Comment Subject: Re: [kriswiner/MPU-9250] calibration routine returns mag biases of zero (#71)

Reopened #71 https://github.com/kriswiner/MPU-9250/issues/71 .

You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/71#event-763853251 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qh-AQrmh4z-4PLdsT-Yrl dRAtEq_ks5qikySgaJpZM4JqbKo . https://github.com/notifications/beacon/AGY1qsVK43z1DoY_j87AX7W9SKp4eE3Rks5 qikySgaJpZM4JqbKo.gif