search

pimoroni / icm20948-python

Python library for the Pimoroni ICM20948 breakout
https://shop.pimoroni.com/products/icm20948
MIT License
50 stars 22 forks source link

Magnetometer never ready with ICM20948 on Waveshare IMX219-83 Stereo Camera #6

Closed leosh64 closed 4 years ago

leosh64 commented 4 years ago

Hello, first of all thanks for providing this great library!

I'm trying to access a ICM20948 on a Waveshare IMX219-83 Stereo Camera using your library. Unfortunately, the magnetometer is never marked as ready, e.g. calling read_magnetometer_data() gets stuck at

        while not self.magnetometer_ready():
            time.sleep(0.00001)

with the following stack trace:

  File "read-all.py", line 20, in <module>
    x, y, z = imu.read_magnetometer_data()
  File "/usr/local/lib/python3.6/dist-packages/icm20948/__init__.py", line 113, in read_magnetometer_data
    while not self.magnetometer_ready():
  File "/usr/local/lib/python3.6/dist-packages/icm20948/__init__.py", line 109, in magnetometer_ready
    return self.mag_read(AK09916_ST1) & 0x01 > 0
  File "/usr/local/lib/python3.6/dist-packages/icm20948/__init__.py", line 93, in mag_read
    self.write(ICM20948_I2C_SLV0_DO, 0xff)
  File "/usr/local/lib/python3.6/dist-packages/icm20948/__init__.py", line 61, in write
    self._bus.write_byte_data(self._addr, reg, value)

Reading accelerometer and gyro data with read_accelerometer_gyro_data works flawlessly though, so the connection to the sensor itself seems fine.

Using C code provided by Waveshare itself, all sensors including the magnetometer can be accessed.

Comparing the register values in Waveshare's C file with the ones in your Python library, I see subtle differences, could they be causing the issue?

/* define ICM-20948 MAG Register  */
#define REG_ADD_MAG_WIA1    0x00
    #define REG_VAL_MAG_WIA1    0x48
#define REG_ADD_MAG_WIA2    0x01
    #define REG_VAL_MAG_WIA2    0x09
#define REG_ADD_MAG_ST2     0x10
#define REG_ADD_MAG_DATA    0x11
#define REG_ADD_MAG_CNTL2   0x31
    #define REG_VAL_MAG_MODE_PD     0x00
    #define REG_VAL_MAG_MODE_SM     0x01
    #define REG_VAL_MAG_MODE_10HZ   0x02
    #define REG_VAL_MAG_MODE_20HZ   0x04
    #define REG_VAL_MAG_MODE_50HZ   0x05
    #define REG_VAL_MAG_MODE_100HZ  0x08
    #define REG_VAL_MAG_MODE_ST     0x10
/* define ICM-20948 MAG Register  end */
Gadgetoid commented 4 years ago

What the Waveshare code calls ST2 is actually defined as ST1 in the datasheet, so the registers are a red herring.

I think the issue might stem from differences between their implementation of reading the magnetometer, and ours.

Theirs is relatively precise in that it only briefly enables communication with the magnetometer, while ours leaves it free-running:

void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8Len, uint8_t *pu8data)
{
    uint8_t i;
    uint8_t u8Temp;

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL,  REG_VAL_REG_BANK_3); //swtich bank3
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_ADDR, u8I2CAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_REG,  u8RegAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN|u8Len);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_USER_CTRL);
    u8Temp |= REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);
    usleep(5*1000);
    u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);

    for(i=0; i<u8Len; i++)
    {
        *(pu8data+i) = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_EXT_SENS_DATA_00+i);

    }
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_I2C_SLV0_CTRL);
    u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN)&(REG_VAL_BIT_MASK_LEN));
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL,  u8Temp);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0

}

Maybe in our case the status register is being cleared to 0 before it's read somehow and hanging.

The weird thing is- this code works for me, so it must be a marginal thing. I would guess some sort of race condition.

I've pushed up a patch branch that brings our read/write functions closer to Waveshare's C, to see if it helps you: https://github.com/pimoroni/icm20948-python/tree/patch-mag-io

You can see a summary of changes here: https://github.com/pimoroni/icm20948-python/compare/patch-mag-io

leosh64 commented 4 years ago

Hey, thanks for the quick fix! I tried your branch patch-mag-io, but unfortunately the magnetometer still doesn't seem to be ready - now the error RuntimeError("Timeout waiting for Magnetometer Ready") is raised, according to your changes. I'll try to look further into this issue.

leosh64 commented 4 years ago

I think I might have found the issue: the Waveshare C code calls the following lines for power management in the init phase:

  I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1,  REG_VAL_ALL_RGE_RESET);
  usleep(10*1000);
  I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1,  REG_VAL_RUN_MODE);

Adding corresponding Python code to the __init__ function makes the magnetometer finally ready:

        self.write(ICM20948_PWR_MGMT_1, 0x80)
        time.sleep(0.01)
        self.write(ICM20948_PWR_MGMT_1, 0x01)

I applied this fix to master and got the magnetometer ready and providing data. Nevertheless, there were some strange "spikes" in the magnetometer output:

Accel: -0.03 -0.96 -0.37
Gyro:  00.70 01.60 -1.64
Mag:   11.25 -26.85 -12.30

Accel: -0.03 -0.97 -0.38
Gyro:  00.51 01.26 -1.26
Mag:   10.35 -26.70 -10.95

Accel: -0.02 -0.97 -0.36
Gyro:  00.22 01.24 -1.24
Mag:   2852.70 2995.20 -2956.95

Accel: -0.03 -0.97 -0.36
Gyro:  00.44 01.28 -1.53
Mag:   09.75 -26.40 -10.05

Accel: -0.03 -0.97 -0.38
Gyro:  00.44 01.31 -1.73
Mag:   11.55 -25.80 -2583.00

Accel: -0.04 -0.97 -0.38
Gyro:  00.01 01.49 -1.40
Mag:   11.55 -25.80 -10.95

Then I applied the fix to your branch patch-mag-io. The spikes were gone and the magnetometer readings seem fine:

Accel: -0.02 -0.97 -0.36
Gyro:  00.45 01.17 -1.32
Mag:   10.35 -26.25 -10.65

Accel: -0.04 -0.96 -0.38
Gyro:  00.07 01.42 -1.15
Mag:   10.35 -26.70 -12.45

Accel: -0.02 -0.97 -0.37
Gyro:  00.50 01.17 -1.66
Mag:   11.10 -26.70 -11.70

Accel: -0.04 -0.97 -0.37
Gyro:  00.18 01.27 -1.12
Mag:   10.50 -25.65 -10.80

Accel: -0.03 -0.97 -0.37
Gyro:  00.29 01.36 -1.40
Mag:   10.20 -25.65 -12.30

Accel: -0.02 -0.97 -0.36
Gyro:  00.19 01.27 -1.12
Mag:   11.25 -25.65 -11.25

So I think both fixes are necessary to make the magnetometer work on the Waveshare device. I've created a pull request here: https://github.com/pimoroni/icm20948-python/pull/7

Gadgetoid commented 4 years ago

You're a legend, thank you for going into this level of sleuthing. I fear without the Waveshare hardware to test on, I wouldn't have come to the same conclusions.