Failure to read magnetometer from DMP FIFO

[bernhardHartleb]

Hello,

we really appreciate the simplification in this library over the original drivers from Invensense.
The hope was to use the DMP for 9-axis sensor fusion with Orientation output referenced to magnetic north.

Using example 6 (Quat9_Orientation) and 9 (MultipleSensors) as a basis, we get stable quaternion output. However, the heading always starts out at zero, and changes in the magnetic field do not seem to matter at all. Digging into this, it is quick to see that there are issues reading the magnetic sensor. If true, this might also explain some other issues posted here.

Steps to reproduce:

• Get the Multi Sensor example 9 to work
• Verify now other hardware or communication issues are present
• Avoid strong magnetic fields nearby which would saturate the magnetometer
• Make the following changes to example 9:

1. Change enableDMPSensor from GAME_ROTATION_VECTOR to ORIENTATION (optional)
2. Enable setDMPODRrate(DMP_ODR_Reg_Cpass, 0)
3. Comment out all other setDMPODRrate calls (optional)

What we would expect is to receive valid magnetometer readings at 55Hz.
The magnetometer was configured to 100Hz continuous mode previously.
I do not know how the sample rate for the external I2C slave (magnetometer) is configured in the DMP.

The actual output is the following:

Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x -677.00  y -256.00  z -103.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x -661.00  y -256.00  z -107.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x -665.00  y -256.00  z -111.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00
Compass:  x 32767.00  y 32767.00  z 32767.00

There are occasional valid readings in the output, but most values are invalid.
Readings that are not 32767 change with the sensor orientation and seem to be the actual magnetic field.

No matter what other options I changed, the DMP keeps posting mostly invalid magnetometer data into the FIFO.
If this is the same data the DMP uses for 9-axis fusion, it is clear why magnetometer is not used in the Quat9 result.
So far, I did not manage to find a different library to cross-reference a possible configuration error.

Please advise if you can reproduce this behaviour.
Thank you!

[PaulZC]

Thank you Bernhard (@bernhardHartleb ),
I will investigate!
Best wishes,
Paul

[PaulZC]

Hi Bernhard (@bernhardHartleb ),
I can confirm that I see the same thing in the Multiple Sensors example. The compass/magnetometer readings only appear about one time in ten...
I see this when using both SPI and I2C, so the issue is not limited only to I2C.
It is making me wonder if I have the compass configured correctly as an I2C peripheral for the DMP.
In the normal (non-DMP) code, we tell the 20948 to read nine bytes from the AK09916 compass, starting with register ST1 (0x10). We also set the mode (rate) to 100Hz.
I wonder if the DMP expects the compass to be configured differently? But as usual, the documentation is not clear and reverse-engineering the InvenSense example code is very difficult.
The DMP does need to be told how fast the compass is running, and we do that by writing to the CPASS_TIME_BUFFER.
I assume the DMP is able to read the data from the AK09916 directly, but I do not know for certain.
I will keep thinking about this, but it is a difficult problem to solve.
Best wishes,
Paul

[PaulZC]

Hi Bernhard,
I have made some progress with this - but I do not have a complete solution yet.
The magnetometer reading does need to be configured differently for the DMP. I found that, when the DMP is enabled, the InvenSense example reads ten bytes from the magnetometer, starting at register 0x03. Register 0x03 is "reserved" and the next nine registers are 'secret' (not mentioned in the datasheet) but the first six seem to contain the raw magnetometer data in big endian format.
By monitoring the auxiliary I2C pins, I can see the traffic between the I2C master and the AK09916. I can see that when the DMP magnetometer data is valid (not 32767) it now matches the magnetometer readings on the I2C bus. The scaling and sign is correct.
This gives me confidence that I am heading in the right direction. But I still do not know why we see so many 32767s.
The modified code is in the release_candidate branch if you have time to take a look. The critical new code is in Example9.
Best wishes,
Paul

[bernhardHartleb]

Hello Paul, thank you so much for your efforts!

The way I (incorrectly) understood this until now was:
After the AK09916 is configured it starts reading magnetometer data into its own internal results registers 0x11 through 0x16 at a frequency of 100Hz. However, the AK specifies that when reading data periodically, the ST2 register (0x18) must be read as at the end so that new measurement data can be release into the registers again. This is equal to I2C block read starting from address 0x10 with a length of 9 bytes. This is exactly what the previous i2cControllerConfigurePeripheral call does. The DMP samples the magnetometer data then independently at 55Hz over I2C. So the effective sampling rate is 55Hz and as long as data is updated fast enough (100Hz). I believe CPASS_TIME_BUFFER is never set in the InvenSense library.

Your new findings about the DMP actually reading from reserved register 0x03 is very interesting!
This puts everything into a different light...

As comparison in Teensy-ICM-20948/src/Icm20948AuxCompassAkm.c this configuration is used when the DMP is enabled.
As you already point out in your code, also in Single Measurement mode.

// HW_AK09916 and DMP on
reg_addr = 0x3;
bytes = 10;
...
/* slave 0 is enabled, read 10 or 8 bytes from here depending on compass type, swap bytes to feed DMP */
result = inv_icm20948_read_secondary(s, COMPASS_I2C_SLV_READ, s->secondary_state.compass_chip_addr, reg_addr, INV_MPU_BIT_GRP | INV_MPU_BIT_BYTE_SW | bytes);
...
/* slave 1 is used to write one-shot accquisition configuration to compass */
/* output data for slave 1 is fixed, single measure mode */
result = inv_icm20948_write_secondary(s, COMPASS_I2C_SLV_WRITE, s->secondary_state.compass_chip_addr, s->secondary_state.mode_reg_addr, DATA_AKM_MODE_SM);
...

DMP slave channel 0 is used for reading data, while channel 1 is used for writing the mode register.
The code above uses inv_icm20948_write_secondary instead of inv_icm20948_execute_write_secondary.

This would mean the DMP cyclicity transitions the Magnetometer from Power Down into Single Measurement Mode and reads the previous results at the DMP sampling rate (55Hz) using two separate I2C slave channels!

I will try out your changes in the release_candidate branch tomorrow and report back the results.

Best regards,
Bernhard

---

In the meantime did manage to get a change in behaviour, but this is obsolete now.
One thing was to disable the temporary SLAVE4 again in register AGB3_REG_I2C_PERIPH4_CTRL after the Magnetometer setup is done. Another finding is that enabling SLV0 bit in AGB3_REG_I2C_MST_DELAY_CTRL and setting DLY > 0 in AGB3_REG_I2C_PERIPH4_CTRL changes how often valid data arrives. Not very useful, since it will reduce the effective sampling rate and 32768 still appears, although not as often.

[PaulZC]

Hi Bernhard,
Thank you for the very useful feedback!
I think I might have found a solution. I was puzzled why, when I monitored the auxiliary I2C pins, I could see that the 20948 was trying to read the magnetometer values at 1125Hz... Way too fast. The AK09916 can only update at a maximum of 100Hz...
I looked again at the wrapped InvenSense code in ZaneL's Teensy library and noticed that they were also setting the I2C_MST_ODR_CONFIG register. I was puzzled by this as setting it should make no difference. But when I set the value to 4, the bus traffic reduced from 1125Hz to the expected ~69Hz. And, wait for it..., the 32767 magnetometer values disappear too!
Please pull the latest release_candidate commits and take a look. I have only had time to update examples 9 and 10. I will do more testing tomorrow and will update the rest of the examples if everything is successful. The critical new code is here.
More tomorrow...
Very best wishes,
Paul

[PaulZC]

Hi Bernhard (@bernhardHartleb ),
Please give v1.2.5 a try. I think this resolves your issue.
Very best wishes,
Paul

[bernhardHartleb]

Hi Paul, yes it is working now!

Every single compass reading is valid (except when saturated).
Orientation output is drifting towards the true geomagnetic heading within a few seconds after initialization.
After that, changes in heading are still instant, meaning the sensor fusion is working.
The examples also got a lot easier to read with default settings.
I am not able to measure the precise speeds and sample rates, but this is issue is solved.

Good work!
Thank you for your quick solution.