Gyro Calibrated Data and Bias

[ibaranov-cp]

Subject of the issue

Hello,
First off, awesome library, thank you very much! I'm integrating it into a project here:
https://github.com/Blueprint-Foundry/Shelly

Your workbench

• What platform are you using? : Custom Dev board
• What version of the device are you using? Is there a firmware version? Latest Firmware downloaded from here :)
• How is the device wired to your platform? I2C line + I2C conditioning circuit, see here for more details: https://workspace.circuitmaker.com/Projects/Details/Ilia-Baranov/Shelly-Dev-Kit
• How is everything being powered? USB from laptop
• Are there any additional details that may help us help you? Power and wiring seem just fine. I am using an ESP32, and use wire with pins 25,23

Steps to reproduce

I edited the DMP_RawAccel example to get Raw Gyro data, so replacing where needed:
INV_ICM20948_SENSOR_RAW_GYROSCOPE
DMP_ODR_Reg_Gyro
DMP_header_bitmap_Gyro
etc.
I then read the XYZ values for gyro, and XYZ bias values instead of the original Accel values.

Expected behaviour

I should be able to read both gyro data and bias data

Actual behaviour

I only get gyro data, bias data is always 0.00. Example:
gyro: X:4.00 Y:14.00 Z:5.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:5.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00

This all started in trying to use the calibrated Gyro field, however that never output data of any kind. Hence I though to use raw data + bias and make my own, but the bias is missing.
I assume there is some configuration step missing?

Request to you awesome folks: Perhaps an example .ino file for Gyro calibrated data and Raw data? :)
(note: all other functions I've tried so far work fine: Quat9, Accel, etc. So I think my chip is healthy since the Quat9 values seem sane.)

[PaulZC]

Hi Ilia (@ibaranov-cp ),
Thank you for raising this issue. Here is what I know about the gyro bias values:
The biases are mentioned in the InvenSense application note (the one we can not share with you):

4.6 Biases
Biases can be set by an outside control. If a bias algorithm is running on the DMP it will over-write these values. The biases are 32-bits in chip frame in hardware unit scaled by 2^12 (FSR 4g) for accel, 2^15 for gyro, in uT scaled by 2^16 for compass.

Register Name Bits[31:0]
GYRO_BIAS_X Gyro X bias
GYRO_BIAS_Y Gyro Y bias
GYRO_BIAS_Z Gyro Z bias
ACCEL_BIAS_X Accel X bias
ACCEL_BIAS_Y Accel Y bias
ACCEL_BIAS_Z Accel Z bias
CPASS_BIAS_X Compass X bias
CPASS_BIAS_Y Compass Y bias
CPASS_BIAS_Z Compass Z bias

Issue #50 has more information about these. But these are the individual bias registers in DMP memory space - not bias data returned via the FIFO.

The gyro biases do not actually get mentioned in the section about FIFO Output. The following are mentioned:

5.2.10 Calibrated Gyro data
Hardware unit scaled by 2^15, 4 bytes for each axis, X, Y and Z in order.
5.2.11 Calibrated Compass data
The unit is uT scaled by 2^16, 4 bytes for each axis, X, Y and Z in order.
5.2.13 Accel Accuracy data
The accuracy is expressed as 0~3. The lowest is 0 and 3 is the highest.
5.2.14 Gyro Accuracy data
The accuracy is expressed as 0~3. The lowest is 0 and 3 is the highest.
5.2.15 Compass Accuracy data
The accuracy is expressed as 0~3. The lowest is 0 and 3 is the highest.

The Data Output Control Register bit flags do not mention the biases, so there is no way to request them explicitly.

They are not mentioned in the Motion Event Control Register either.

They are also not mentioned in the two DMP Header bytes, which precede the data in the FIFO.

We only know they could be returned via the FIFO because they are mentioned in the InvenSense Nucleo example code (which ZaneL wrapped in his Teensy-ICM-20948 library). This line shows where they would be stored in the decoded FIFO data. This line adds GYRO_BIAS_DATA_SZ (SiZe) to the expected byte count. These two lines perform the data extraction from the FIFO packet. Our code does the same thing. Note that the bias data is appended to the raw gyro data, not the calibrated data.

Another piece of weirdness is this. The InvenSense code suggests that the calibrated gyro data set is not supported as GYRO_CALIBR_SET is not mentioned in inv_icm20948_inv_decode_one_ivory_fifo_packet. In our code, we skip over the Gyro_Calibr data here. IIRC I confirmed that we needed to do this by requesting the calibrated gyro data and then checking that it was not actually returned in the FIFO data (even though the DMP_header_bitmap_Gyro_Calibr bit was set in the header!).

Long story short - I think we are doing everything we can to try and get the gyro bias data to you. But the DMP seems to insist on returning zeros (just like in #50).

If you find a solution, please let me know.

Best wishes,
Paul

[PaulZC]

Just to prove I'm not making up the part about the Gyro Calibr set not being supported... Here are the notes I took at the time.
I captured a FIFO packet. The bits in the Header word (E4 40) define that the packet contains Accel, Gyro, Compass, Quat9 and Gyro_Calibr data. Adding up the length of each data field, the packet should have been 54 bytes long. But was in fact only 42 bytes long. The Gyro_Calibr data was not present - even though the Header word says it should be. This was what led me to examine the InvenSense code very closely and then comment the Gyro_Calibr extraction in our code.

[ibaranov-cp]

Thank you so much @PaulZC for diving into this detail. I don't know I have much to add, but I will take a deep dive myself and see if there is anything useful I can contribute. Not likely though, your explanation and details above make perfect sense :)

[ibaranov-cp]

Well something very interesting is going on.

I added the just lines shown in #50, no other changes, to my FIFO data processing section:

if ((data.header & DMP_header_bitmap_Gyro) > 0) // Check for calibrated Gyro data
      {
        unsigned char gyroBiasX[4]; // Big-endian
        boolean success = (IMU.readDMPmems(GYRO_BIAS_X, 4, &gyroBiasX[0]) == ICM_20948_Stat_Ok);
        //Serial.println(gyroBiasX[0]);

        float gyro_x = (float)data.Raw_Gyro.Data.X;
        float gyro_y = (float)data.Raw_Gyro.Data.Y;
        float gyro_z = (float)data.Raw_Gyro.Data.Z;
        float gyro_xb = (float)data.Raw_Gyro.Data.BiasX;
        float gyro_yb = (float)data.Raw_Gyro.Data.BiasY;
        float gyro_zb = (float)data.Raw_Gyro.Data.BiasZ;
 
        Serial.print(F("gyro: X:"));
        Serial.print(gyro_x);
        Serial.print(F(" Y:"));
        Serial.print(gyro_y);
        Serial.print(F(" Z:"));
        Serial.print(gyro_z);
        Serial.print(F(" XB:"));
        Serial.print(gyro_xb);
        Serial.print(F(" YB:"));
        Serial.print(gyro_yb);
        Serial.print(F(" ZB:"));
        Serial.println(gyro_zb);
        //Serial.println(IMU.temp());
      }

I then shake/twist (very quickly) and tap the board on the table, and actually seem to get some data:

gyro: X:-3.00 Y:3.00 Z:15.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:1.00 Y:4.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-178.00 Y:0.00 Z:0.00 XB:0.00 YB:-4400.00 ZB:-22260.00
gyro: X:0.00 Y:0.00 Z:0.00 XB:2236.00 YB:4908.00 ZB:-293.00
gyro: X:25622.00 Y:304.00 Z:12302.00 XB:64.00 YB:12303.00 ZB:-15360.00
gyro: X:4029.00 Y:1029.00 Z:162.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:99.00 Y:-256.00 Z:454.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:12303.00 Y:64.00 Z:12302.00 XB:-15360.00 YB:352.00 ZB:2364.00
gyro: X:744.00 Y:-2094.00 Z:311.00 XB:4111.00 YB:64.00 ZB:4111.00
gyro: X:4111.00 Y:64.00 Z:4111.00 XB:-15296.00 YB:98.00 ZB:-58.00
gyro: X:335.00 Y:12303.00 Z:72.00 XB:4096.00 YB:2.00 ZB:12302.00
gyro: X:-865.00 Y:68.00 Z:-951.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:599.00 Y:949.00 Z:-887.00 XB:-9706.00 YB:530.00 ZB:24701.00
gyro: X:-13.00 Y:-12.00 Z:-412.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:17.00 Z:6.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-2.00 Y:3.00 Z:10.00 XB:0.00 YB:0.00 ZB:0.00

Perhaps by reading bias registers, we are setting some secret internal flag in the device??
I tried also placing this after the initialization of the DMP just once at the start, but device doesn't publish zb values that way, seems it needs to be read frequently for this to work... very odd.

[ibaranov-cp]

It gets even wilder @PaulZC (again, I am unsure if this is a wild goose chase and I am just reading garbage...)

I tried the above trick for gyro calibrated as well, uncommented the lines you pointed here:
https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary/blob/master/src/util/ICM_20948_C.c#L2106-L2129

and got:

gyroC: X:58065000.00 Y:1376245.00 Z:7405780.00
gyroC: X:13893909.00 Y:-100261312.00 Z:192606192.00
gyro: X:22099.00 Y:389.00 Z:12303.00 XB:-15296.00 YB:94.00 ZB:-464.00
gyro: X:28471.00 Y:389.00 Z:12303.00 XB:-15296.00 YB:88.00 ZB:-302.00
gyroC: X:-576845376.00 Y:-803446976.00 Z:23080974.00
gyro: X:-28989.00 Y:383.00 Z:12302.00 XB:-15296.00 YB:-30.00 ZB:686.00
gyro: X:122.00 Y:-4.00 Z:882.00 XB:-459.00 YB:227.00 ZB:600.00
gyroC: X:-35173896.00 Y:-881476544.00 Z:21704720.00
gyroC: X:806339648.00 Y:983014.00 Z:68550672.00
gyro: X:-4.00 Y:6.00 Z:16.00 XB:113.00 YB:212.00 ZB:277.00
gyroC: X:-25098532.00 Y:-883171520.00 Z:21704720.00
gyroC: X:-20488538.00 Y:-886452416.00 Z:21704720.00

[ibaranov-cp]

One more strange data point:

I've now commented back out all my changes (both to the ICM_20948_C.c library, gyro_calibrated related items, and the reading the register change). I also completely pulled power from the board to ensure a clean slate.

Now running just the Raw Gyro data as I did before, the IMU starts publishing bias data after ~20 seconds of sitting on the table:

IMU Status: All is well.
DMP enabled!
gyro: X:6.00 Y:11.00 Z:16.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:2.00 Z:10.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:16.00 Z:18.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:0.00 Y:10.00 Z:2.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:5.00 Z:11.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:4.00 Y:14.00 Z:3.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:16.00 Z:6.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:6.00 Y:11.00 Z:15.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:11.00 Z:14.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:1.00 Y:-2.00 Z:17.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:2.00 Y:9.00 Z:2.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:9.00 Z:8.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-5.00 Y:8.00 Z:16.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:9.00 Y:17.00 Z:5.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:5.00 Z:16.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:15.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:4.00 Y:3.00 Z:12.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:9.00 Z:8.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-5.00 Y:11.00 Z:10.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-6.00 Y:9.00 Z:14.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:18.00 Y:1.00 Z:11.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:5.00 Y:8.00 Z:9.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:10.00 Y:14.00 Z:13.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:5.00 Y:10.00 Z:9.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-5.00 Y:0.00 Z:8.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:6.00 Y:3.00 Z:9.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:5.00 Z:10.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:6.00 Z:12.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:1.00 Y:10.00 Z:8.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:2.00 Y:9.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:10.00 Z:10.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:9.00 Z:9.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:5.00 Y:12.00 Z:6.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:8.00 Y:-2.00 Z:12.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:4.00 Y:7.00 Z:14.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-5.00 Y:13.00 Z:12.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-2.00 Y:6.00 Z:1.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:2.00 Y:6.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:4.00 Y:14.00 Z:11.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:5.00 Y:7.00 Z:6.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:5.00 Z:14.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:10.00 Y:-1.00 Z:12.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:11.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:0.00 Y:14.00 Z:7.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:7.00 Y:10.00 Z:20.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:1.00 Y:5.00 Z:13.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:3.00 Y:9.00 Z:3.00 XB:0.00 YB:0.00 ZB:0.00
gyro: X:-1.00 Y:9.00 Z:5.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:3.00 Y:12.00 Z:9.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:3.00 Y:4.00 Z:9.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-6.00 Y:10.00 Z:10.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:2.00 Y:4.00 Z:13.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:1.00 Y:14.00 Z:13.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:3.00 Y:9.00 Z:8.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:7.00 Y:10.00 Z:17.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:4.00 Y:8.00 Z:8.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:9.00 Y:7.00 Z:10.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:9.00 Y:9.00 Z:6.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:3.00 Y:6.00 Z:8.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:11.00 Y:8.00 Z:9.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-183.00 Y:-57.00 Z:-223.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-6.00 Y:20.00 Z:7.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-121.00 Y:63.00 Z:9.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-1.00 Y:-1.00 Z:15.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-1.00 Y:6.00 Z:13.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:1.00 Y:9.00 Z:14.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:6.00 Y:11.00 Z:14.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:4.00 Y:7.00 Z:7.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-1.00 Y:7.00 Z:6.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:9.00 Y:12.00 Z:15.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:2.00 Y:14.00 Z:18.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-7.00 Y:18.00 Z:15.00 XB:85.00 YB:252.00 ZB:321.00
gyro: X:-5.00 Y:7.00 Z:9.00 XB:85.00 YB:252.00 ZB:321.00

And these values are not some static zombie register, after a few more minutes the values have changed:

gyro: X:4.00 Y:-5.00 Z:4.00 XB:59.00 YB:244.00 ZB:334.00
gyro: X:-3.00 Y:9.00 Z:15.00 XB:59.00 YB:244.00 ZB:334.00
gyro: X:7.00 Y:11.00 Z:9.00 XB:59.00 YB:244.00 ZB:334.00
gyro: X:3.00 Y:8.00 Z:9.00 XB:59.00 YB:244.00 ZB:334.00
gyro: X:3.00 Y:-1.00 Z:12.00 XB:59.00 YB:244.00 ZB:334.00

And my code is now just (ignore the json part, that for my application)

 if ((data.header & DMP_header_bitmap_Gyro) > 0) // Check for callibrated Gyro data
      {
        //unsigned char gyroBiasX[4]; // Big-endian
        //boolean success = (IMU.readDMPmems(GYRO_BIAS_X, 4, &gyroBiasX[0]) == ICM_20948_Stat_Ok);

        float gyro_x = (float)data.Raw_Gyro.Data.X;
        float gyro_y = (float)data.Raw_Gyro.Data.Y;
        float gyro_z = (float)data.Raw_Gyro.Data.Z;
        float gyro_xb = (float)data.Raw_Gyro.Data.BiasX;
        float gyro_yb = (float)data.Raw_Gyro.Data.BiasY;
        float gyro_zb = (float)data.Raw_Gyro.Data.BiasZ;

        json_Publish_IMU["msg"]["angular_velocity"]["x"] = gyro_x;
        json_Publish_IMU["msg"]["angular_velocity"]["y"] = gyro_y; //rad/s
        json_Publish_IMU["msg"]["angular_velocity"]["z"] = gyro_z;

        Serial.print(F("gyro: X:"));
        Serial.print(gyro_x);
        Serial.print(F(" Y:"));
        Serial.print(gyro_y);
        Serial.print(F(" Z:"));
        Serial.print(gyro_z);
        Serial.print(F(" XB:"));
        Serial.print(gyro_xb);
        Serial.print(F(" YB:"));
        Serial.print(gyro_yb);
        Serial.print(F(" ZB:"));
        Serial.println(gyro_zb);
        //Serial.println(IMU.temp());
      }

[ibaranov-cp]

Code now checked in, the example here is large since it contains a bunch of stuff you don't care about from my project, but it could be helpful:
Blueprint-Foundry/Shelly@be441b7

[PaulZC]

Hi Ilia (@ibaranov-cp ),
Thank you for investigating this.
The DMP starting to return the bias data after ~20 seconds is probably not a complete surprise. We know that the DMP goes through an internal calibration at start-up. It needs to figure out where magnetic north is for example, when using any of the Quat9 / Geomagnetic orientation modes. So, it does seem to be working as 'expected'?
Sadly, the GYRO_CALIBR data will be, as you suspected, just zombie data. It will just be whatever garbage is in the RAM locations beyond the end of the packet. The first two bytes will be 'valid', those are the two footer bytes, but the rest will just be noise.
I will leave this issue open for a while - especially as I need to cross-link it to #50.
Good luck with your project!
Paul

[PaulZC]

Hi Ilia (@ibaranov-cp ),

Please try version 1.2.6 of the library. I have made a small but important change which is needed to allow the DMP to work correctly. I will be interested to know if you are now able to record the bias data?

Best wishes,
Paul

[ibaranov-cp]

Hello,

My apologies for the slow response, I've tested with 1.2.7 and have gotten similar bias values that I did before from the raw gyro from Raw_Gyro.Data.BiasX, Raw_Gyro.Data.BiasY and Raw_Gyro.Data.BiasZ
So we are looking good :)

The Gyro Calibrated values are still not available, but from your earlier comments that does seem expected.

I think we are good to close this issue

[PaulZC]

Thank you Ilia (@ibaranov-cp ),
Closing...
Best wishes,
Paul