IMU scaling depends on board revision

[dpkoch]

@superjax So I flashed the latest firmware on both a rev5 and a rev2 Flip32 board. When streaming IMU over mavlink, the accel scale factor in fcu_io2 rev5 board is correct (I get 9.8m/s^2 at rest). The scale factor is off by 2 (I get 4.9m/s^2 at rest) for the rev2 board.

The solution to this problem may depend on fcu_io2, but it's also possible that this could be affecting onboard estimation. On the fcu_io2 side we could imlement some way to retrieve the scale factor from the naze. Another possibly is to send scaled values over mavlink instead, although I'm not sure what the answer is there based on our previous discussion.

Either way, we'll need to ensure that the accel is being scaled correctly on board when necessary (e.g. for altitude estimation). We may already have what we need from the mpu6050_init, but we'll need to check.

[superjax]

This should be linked to byu-magicc/fcu_io#2

[dpkoch]

I have confirmed that _accel_scale is at least close to correct for both the rev2 (4807) and rev5 (2394) boards. The rev5 value matches what we've been using in fcu_io. I would expect them to be off by a factor of 2, but they're not quite, so that's a little bit weird.

[superjax]

This is definitely a problem with `mpu6050_init' in Breezy. I believe that it has something to do with the 'half' business going on in the initialization.

[superjax]

The true _accel_scale should be 9.807/4096 or 0.002394. This should be true for both boards. They both have a 8G scale on a 16 bit ADC, and the datasheet says that the scale should be 4096LSB/g. The fact that it's different between boards is weird.

[superjax]

I may have fixed a bug in mpu6050_init. Here is the relevant parts of the datasheet


and here is the original code

enum accel_fsr_e {
    INV_FSR_2G = 0,
    INV_FSR_4G,
    INV_FSR_8G,
    INV_FSR_16G,
    NUM_ACCEL_FSR
}; 
...
mpuWriteRegisterI2C(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

This results in some odd behaviour, instead of getting 4096LSB/g as prescribed in the datasheet, I was getting 2048.

However, if I add a mask to zero out everything except the relevant AFS_SEL bits

mpuWriteRegisterI2C(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3 & 0x18);

I get the 4096LSB/g as prescribed in the datasheet.

[superjax]

Okay, trying to understand some more. Here is the code which determines the gyro scale factor.

*gyroScale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

Where the heck is the 4 coming from? and where is the 0.000001f coming from too? Here is the table from the datasheet, which is located here

From what I gather from the datasheet, it should be

 *gyroScale = (3.14159/180.0)/16.4; 

Any ideas?

[superjax]

Okay, I have confirmed that the data coming from both a rev5 and a rev2 board should actually be the same. At least, the two boards I have. Would you mind running the accelgyro example from my github on some of your boards? My two boards print raw units in the same scale (4096 LSB/g). I didn't let the temperature settle out, I was mostly looking for the LSB/g order

Here is my rev2 board (the blue flip32+)

accel_scale  gyro_scale  acc[0]  acc[1] acc[2]  gyr[0] gyr[1] gyr[2] temp   dt
2394         4           267     -274   4143    22     -5     -8     -669   998

And this is my rev5 board (the black flip32+)

accel_scale  gyro_scale  acc[0]  acc[1] acc[2]  gyr[0] gyr[1] gyr[2] temp   dt
2394         4           146     6      3973    -8     -5     6      -830   999

And this is another rev5 board (the white naze32 on the hummingbird)

accel_scale  gyro_scale  acc[0]  acc[1] acc[2]  gyr[0] gyr[1] gyr[2] temp   dt
2394         4           33      -37    4519    0      0      1      -890   1001

[dpkoch]

Okay I ran the accelgyro example from your master branch (is that right?) on a rev2 (blue flip32):

accel_scale  gyro_scale  acc[0]  acc[1]  acc[2]  gyr[0]  gyr[1]  gyr[2] temp  dt
4807         4           119     -65     1809    -13    -10     -5      -230  1000

So my scale factor still seems to be off.

I'm working on trying it with the rev5 board, but I'm unable to flash it at the moment for some reason...

[dpkoch]

Here's the output I get from a rev5 board (black flip32):

accel_scale  gyro_scale  acc[0]  acc[1]  acc[2]  gyr[0]  gyr[1]  gyr[2] temp  dt
2394         4           88      -68     3809    -3      0       0      -394  1004

I double checked, and I did call the mpu6050_init function with the correct board revision for both cases.

[dpkoch]

Alright so it looks like the bit mask you talked about in your comment above wasn't in there. I added it in, but it still didn't seem to help. Here's the new output:

rev2:

4807    4   120 -53 1796    -12 -11 -4  -336    1000

rev5:

2394    4   122 -104    3819    -3  0   1   -188    1004

[superjax]

That's really weird. I wonder why your rev2 has a different scale than all of the other boards we've tested. For some reason, your rev2 board is getting set to +/- 16g rather than +/-8g. Can you tell if the half variable is getting set to true? (Try just throwing an LED0_ON somewhere in the mpu6050_init function of drv_mpu6050.c

[dpkoch]

Yes, I have confirmed that half gets set on my rev2 board, but not my rev5 board.

Also, accel_scale is still close to correct on my rev2 board (when I stream the scaled imu as floats it is close to right, but the magnitude is a little bigger than it should be). I think we would expect it to be 2394*2 = 4788 rather than the 4807 that gets set right now?

[superjax]

That's right. I wonder if it's a truncation error. I can't read the lettering on the actual chip on my blue board, but I think it might have been washed off from spraying it with freon so many times. Do you think you can see if there is any difference in the MPU6050 chip between your boards? Based on the code, it appears that there are different revisions of MPU6050.

[superjax]

So I think this is the problem.

in drv_mpu6050.c we see first,

*acc1G = 512 * 8;

Then, it decides which revision of board you have, and if half is true...

if (half)
        *acc1G = 255 * 8;

The important thing is that 512*8 = 4096, while 255*8 = 2040. Not 2048 which is what you would expect. _accel_scale is later calculated by converting the LSB/g conversion (acc1G) to mm/s^2/LSB, which would is

_accel_scale = (1000*9807)/acc1G; // convert to um/s^2

The difference in acc1G is probably causing the issue. I bet if we change it to 256*8 instead of 255*8 it will solve the problem

[superjax]

I just pushed a change to my master branch of Breezy. Do you think you could test it before I try to pull it from Simon? I am unable to replicate the bug, but I adjusted the lines I talked about above.

[dpkoch]

Okay I tested it and it seems to help. Streaming the values on my rev2 board the bias is way off (no surprise there), but the scale seems to be correct. The difference between the z accel value sitting on my desk and then upside down is very close to 2g. I don't have a way to check that accurately, but it seems pretty good and your change makes sense to me.

Looking through the code it looks like this is an issue of the revision of the MPU6050 itself, not the Naze32 per se, which may be why you can't replicate it with your rev2 if your board came from a different batch? Also I guess with these older boards the accel will be operating with a different full-scale range, but as long as we know about I suppose we can accept that. However, if we do accept that then it probably makes an additional point for the case of converting values to SI units or at least scaled values as quickly as possible rather than trying to keep track of the scale factor for too long, especially over mavlink, since it's not constant between boards.

[superjax]

so is this closed?

[dpkoch]

If we're okay with the onboard accel range being different, on some boards, then yes I think it is. We're still getting it in the right units, it's just a question of if the sensitivity is acceptable.

[dpkoch]

Also, were we going to go with the change in BreezySTM32 (acc1G = 256 * 8 instead of acc1G = 255 * 8)?

[superjax]

I think that's the answer. It's in the official repository, and it is working, so let's go ahead and keep it. (Do we need to update the submodule?)

[dpkoch]

Right. Nope it's good, the submodule is up to date. Let's consider this issue closed.