kriswiner / MPU9250

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

Initial values in accelerometer inconsitent between restarts #77

Open multivac61 opened 8 years ago

multivac61 commented 8 years ago

Hi Kris!

Thank you so much for making your work public so I lazy users like myself don't have to implement this read off code. I am currently running your code and I get very weird and inconsistent initial values from the accelerometer. The picture below shows you what I am walking about. Where there are discontinuities in the accelerometer reads I restarted my MCU and thus ran the calibrate and initialize code you wrote. It should be noted that I am not displaying the data with the bias removed but it is anyway much less than 180 mg.

accelerometer_init_values

After the initial values have settled then the readings are constant if I don't move my device around. My MPU9250 module is taped to my desk which is fairly well leveled so I would expect the accelerometer readings to be approximately (0, 0, -1) [x, y, z].

multivac61 commented 8 years ago

Hmm.. I think I might have found the culprit. Removing the code that pushes accelerometer biases to hardware registers made the inconsistencies go away. Now the accelerometer output has a consistent bias which should be easy to deal with.

accelerometer_init_values-no_bias

kriswiner commented 8 years ago

This is why I don't use the hardware accel bias registers. You have to handle the temp correction bit properly and I am not sure how to do this. The gyro hardware bias registers work fine.

-----Original Message----- From: multivac61 [mailto:notifications@github.com] Sent: September 26, 2016 7:34 AM To: kriswiner/MPU-9250 Subject: Re: [kriswiner/MPU-9250] Initial values in accelerometer inconsitent between restarts (#77)

Hmm.. I think I might have found the culprit. Removing the code that pushes accelerometer biases to hardware registers made the inconsistencies go away. Now the accelerometer output has a consistent bias which should be easy to deal with.

accelerometer_init_values-no_bias https://cloud.githubusercontent.com/assets/4436129/18838140/d75b6f10-83f5-1 1e6-9d57-40a7388ca2f9.png

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/77#issuecomment-249587419 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qpAvN5UJbg_-BGhBu1g_L PQFozdbks5qt9e8gaJpZM4KGipd . https://github.com/notifications/beacon/AGY1qr2F2CujwnVojt9P7LX-C1f9JnaZks5 qt9e8gaJpZM4KGipd.gif

MikeFair commented 7 years ago

I think I figured out what's going on. There's a few things.

One, I noticed the reg was declared as a 32-bit int32_t accel_bias_reg value; perhaps on purpose (I could think of some reasons) but it's obviously a 16-bit field.

I'm not sure how this was working on the 6050, but the bias is a 15-bit value starting at bit 1 in the _L register. (In other words, it's a 15-bit value stuck in the high 16-bits of a two byte field.)

When the 9250 powers up, those registers contain the factory default values but do not seem to take an "offset value" when you write them. So the "subtract the factory default" code seems to be a mistake on this chip (perhaps they internalized that piece of the math logic directly on the chip now?). Also, the temp_calibrate bit is now a reserved bit on the MPU9250 and doesn't need to be tracked.

Which means, there's no requirement to fetch the factory preset at all; calculate the right value and set it.

Consequently, that entire code can now mimic the gyro code.

Except for the bit shift. After issuing the accel_bias_reg[i] = accel_bias[i]/8; it needs to do an accel_bias_reg[i] = accel_bias_reg[i] << 1 before writing up the bytes. I guess technically it could do that all in a single line accel_bias_reg[i] = (int16_t)((accel_bias[i]/8) << 1);.

FWIW, if you were going to attempt reading the factory presets, make sure to shift the value after fetching it; otherwise the value is going to read double the actual value.

MikeFair commented 7 years ago

I was mistaken about:

Which means, there's no requirement to fetch the factory preset at all; calculate the right value and set it.

I understand now, they are additive, the initial measurements received had these initial offsets baked into them and so the new offsets "include" those initial values; however they are 'additive' so the math there needs to be a += not a -=. My own testing bears this out; changing the math to a + and using the bit shift code from above, this calibration method computes around the same values on the MPU9250 I'm testing as some other methods I've used/tested.


However, if you reset the microcontroller without cutting all power completely, the MPU will "remember" the last stored values (even after the hard reset the calibration function issues)... This means that if you run calibrate again (which because you just reset the MCU should be one of the next things to happen), the Gyros now also have an equivalent "factory preset" value (the one from the last calibrate call).

It seems leaving even just the clock and data lines alone (pulling V+ and Ground) are enough to keep the memory "warm". [And I know from other experiences that you can almost power the entire unit just on the V+ being fed to the ADO line.]

It would seem the safest course of action is to always pull all the values, calculate new offsets, add them to the "presets" (even if the presets were 0 like the Gyros are on startup), and put then put them back.