Mag support for SPI. Implement mag over I2C master rather than bare I2C.

I believe the primary issue with with getting the mag to work in original library was two fold: the bank bits were not being set correctly, and bank3 was not set in ICM_20948_i2c_master_configure_slave().

Data from mag is now harvested at the same time at gyro and accel from EXT_SLV_SENS_DATA_xx registers.

* Add i2cMasterReset() - Needed to reset the I2C master during startup in the case that it has hung due to an incomplete previous transaction with Mag.
* Remove all I2C based mag functions. Move to global ICM class.
* Remove _has_magnetometer flag. No longer needed. If mag doesn't come online, data returned in 0 in DMP array.

Author: Nathan Seidle

src/ICM_20948.cpp

@@ -18,11 +18,6 @@ ICM_20948_AGMT_t ICM_20948::getAGMT(void)
 {
     status = ICM_20948_get_agmt(&_device, &agmt);
 
-    if (_has_magnetometer)
-    {
-        getMagnetometerData(&agmt);
-    }
-
     return agmt;
 }
 
@@ -527,7 +522,6 @@ ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable)
     }
     if (en.RAW_DATA_0_RDY_EN != enable)
     {
-        Serial.println("mismatch error");
         status = ICM_20948_Stat_Err;
         return status;
     }
@@ -589,6 +583,12 @@ ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable)
     return status;
 }
 
+ICM_20948_Status_e ICM_20948::i2cMasterReset()
+{
+    status = ICM_20948_i2c_master_reset(&_device);
+    return status;
+}
+
 ICM_20948_Status_e ICM_20948::i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap)
 {
     status = ICM_20948_i2c_master_configure_slave(&_device, slave, addr, reg, len, Rw, enable, data_only, grp, swap);
@@ -600,9 +600,10 @@ ICM_20948_Status_e ICM_20948::i2cMasterSLV4Transaction(uint8_t addr, uint8_t reg
     status = ICM_20948_i2c_master_slv4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr);
     return status;
 }
-ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data)
+
+ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t *data)
 {
-    status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data);
+    status = ICM_20948_i2c_master_single_w(&_device, addr, reg, data);
     return status;
 }
 uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg)
@@ -684,40 +685,16 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         status = retval;
         return status;
     }
-
-    _has_magnetometer = true;
     retval = startupMagnetometer();
-    if ((retval != ICM_20948_Stat_Ok) && (retval != ICM_20948_Stat_NotImpl))
+    if (retval != ICM_20948_Stat_Ok)
     {
         status = retval;
         return status;
     }
-    if (retval == ICM_20948_Stat_NotImpl)
-    {
-        // This is a temporary fix.
-        // Ultimately we *should* be able to configure the I2C master to handle the
-        // magnetometer no matter what interface (SPI / I2C) we are using.
-
-        // Should try testing I2C master functionality on a bare ICM chip w/o TXS0108 level shifter...
-
-        _has_magnetometer = false;
-        retval = ICM_20948_Stat_Ok; // reset the retval because we handled it in this cases
-    }
 
-    status = retval;
     return status;
 }
 
-ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
-{
-    return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer
-}
-
-ICM_20948_Status_e ICM_20948::getMagnetometerData(ICM_20948_AGMT_t *pagmt)
-{
-    return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer
-}
-
 // direct read/write
 ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len)
 {
@@ -731,6 +708,18 @@ ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len)
     return (status);
 }
 
+uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg)
+{
+    uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg);
+    return data;
+}
+
+ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
+{
+    status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, pdata);
+    return status;
+}
+
 // I2C
 ICM_20948_I2C::ICM_20948_I2C()
 {
@@ -780,103 +769,78 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
     return status;
 }
 
-ICM_20948_Status_e ICM_20948_I2C::startupMagnetometer(void)
+ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
 {
-    // If using the magnetometer through passthrough:
-    i2cMasterPassthrough(true); // Set passthrough mode to try to access the magnetometer (by default I2C master is disabled but you still have to enable the passthrough)
+    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-    // Try to set up magnetometer
-    AK09916_CNTL2_Reg_t reg;
-    reg.MODE = AK09916_mode_cont_100hz;
+    i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL
+    i2cMasterEnable(true);
 
-    ICM_20948_Status_e retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)&reg, sizeof(AK09916_CNTL2_Reg_t));
-    status = retval;
-    if (status == ICM_20948_Stat_Ok)
+    //After a ICM reset the Mag sensor may stop responding over the I2C master
+    //Reset the Master I2C until it responds
+    uint8_t tries = 0;
+    uint8_t maxTries = 5;
+    while (tries < maxTries)
     {
-        _has_magnetometer = true;
-    }
-    return status;
-}
+        //See if we can read the WhoIAm register correctly
+        retval = magWhoIAm();
+        if (retval == ICM_20948_Stat_Ok)
+            break; //WIA matched!
 
-ICM_20948_Status_e ICM_20948_I2C::magWhoIAm(void)
-{
-    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-
-    const uint8_t len = 2;
-    uint8_t whoiam[len];
-    retval = readMag(AK09916_REG_WIA1, whoiam, len);
-    status = retval;
-    if (retval != ICM_20948_Stat_Ok)
-    {
-        return retval;
+        i2cMasterReset(); //Otherwise, reset the master I2C and try again
+        tries++;
     }
 
-    if ((whoiam[0] == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam[1] == (MAG_AK09916_WHO_AM_I & 0xFF)))
+    if (tries == maxTries)
     {
-        retval = ICM_20948_Stat_Ok;
-        status = retval;
+        status = ICM_20948_Stat_WrongID;
         return status;
     }
-    retval = ICM_20948_Stat_WrongID;
-    status = retval;
-    return status;
-}
 
-bool ICM_20948_I2C::magIsConnected(void)
-{
-    if (magWhoIAm() != ICM_20948_Stat_Ok)
+    //Serial.printf("Mag connected tries: %d\n", tries);
+
+    //Set up magnetometer
+    AK09916_CNTL2_Reg_t reg;
+    reg.MODE = AK09916_mode_cont_100hz;
+    retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)&reg);
+    if (retval != ICM_20948_Stat_Ok)
     {
-        return false;
+        status = retval;
+        return status;
     }
-    return true;
-}
 
-ICM_20948_Status_e ICM_20948_I2C::getMagnetometerData(ICM_20948_AGMT_t *pagmt)
-{
-
-    const uint8_t reqd_len = 9; // you must read all the way through the status2 register to re-enable the next measurement
-    uint8_t buff[reqd_len];
-
-    status = readMag(AK09916_REG_ST1, buff, reqd_len);
-    if (status != ICM_20948_Stat_Ok)
+    retval = i2cMasterConfigureSlave(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false);
+    if (retval != ICM_20948_Stat_Ok)
     {
+        status = retval;
         return status;
     }
 
-    pagmt->mag.axes.x = ((buff[2] << 8) | (buff[1] & 0xFF));
-    pagmt->mag.axes.y = ((buff[4] << 8) | (buff[3] & 0xFF));
-    pagmt->mag.axes.z = ((buff[6] << 8) | (buff[5] & 0xFF));
-
     return status;
 }
 
-ICM_20948_Status_e ICM_20948_I2C::readMag(uint8_t reg, uint8_t *pdata, uint8_t len)
+ICM_20948_Status_e ICM_20948::magWhoIAm(void)
 {
-    _i2c->beginTransmission(MAG_AK09916_I2C_ADDR);
-    _i2c->write(reg);
-    _i2c->endTransmission(false);
+    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-    uint8_t num_received = _i2c->requestFrom((uint8_t)MAG_AK09916_I2C_ADDR, (uint8_t)len);
-    if (num_received != len)
+    uint8_t whoiam1, whoiam2;
+    whoiam1 = readMag(AK09916_REG_WIA1);
+    whoiam2 = readMag(AK09916_REG_WIA2);
+    status = retval;
+    if (retval != ICM_20948_Stat_Ok)
     {
-        return ICM_20948_Stat_NoData;
+        return retval;
     }
 
-    for (uint8_t indi = 0; indi < len; indi++)
+    if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF)))
     {
-        *(pdata + indi) = _i2c->read();
+        retval = ICM_20948_Stat_Ok;
+        status = retval;
+        return status;
     }
-
-    return ICM_20948_Stat_Ok;
-}
-
-ICM_20948_Status_e ICM_20948_I2C::writeMag(uint8_t reg, uint8_t *pdata, uint8_t len)
-{
-    _i2c->beginTransmission(MAG_AK09916_I2C_ADDR);
-    _i2c->write(reg);
-    _i2c->write(pdata, len);
-    _i2c->endTransmission();
-    return ICM_20948_Stat_Ok; // todo: check return of 'endTransmission' to verify all bytes sent w/ ACK
+    retval = ICM_20948_Stat_WrongID;
+    status = retval;
+    return status;
 }
 
 // SPI
@@ -925,8 +889,6 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
         return status;
     }
 
-    // todo: disable I2C interface to prevent accidents
-
     return ICM_20948_Stat_Ok;
 }
 

src/ICM_20948.h

@@ -8,6 +8,7 @@ A C++ interface to the ICM-20948
 #define _ICM_20948_H_
 
 #include "util/ICM_20948_C.h" // The C backbone
+#include "util/AK09916_REGISTERS.h"
 
 #include "Arduino.h" // Arduino support
 #include "Wire.h"
@@ -21,7 +22,6 @@ class ICM_20948
 private:
 protected:
     ICM_20948_Device_t _device;
-    bool _has_magnetometer;
 
     float getTempC(int16_t val);
     float getGyrDPS(int16_t axis_val);
@@ -92,20 +92,28 @@ class ICM_20948
     // Interface Options
     ICM_20948_Status_e i2cMasterPassthrough(bool passthrough = true);
     ICM_20948_Status_e i2cMasterEnable(bool enable = true);
-    ICM_20948_Status_e i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false);
+    ICM_20948_Status_e i2cMasterReset();
 
+    //Used for configuring slaves 0-3
+    ICM_20948_Status_e i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false);
     ICM_20948_Status_e i2cMasterSLV4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr = true);
-    ICM_20948_Status_e i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data);
+
+    //Used for configuring the Magnetometer
+    ICM_20948_Status_e i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t *data);
     uint8_t i2cMasterSingleR(uint8_t addr, uint8_t reg);
 
     // Default Setup
     ICM_20948_Status_e startupDefault(void);
-    virtual ICM_20948_Status_e startupMagnetometer(void);
-    virtual ICM_20948_Status_e getMagnetometerData(ICM_20948_AGMT_t *pagmt);
 
     // direct read/write
     ICM_20948_Status_e read(uint8_t reg, uint8_t *pdata, uint32_t len);
     ICM_20948_Status_e write(uint8_t reg, uint8_t *pdata, uint32_t len);
+
+    //Mag specific
+    ICM_20948_Status_e startupMagnetometer(void);
+    ICM_20948_Status_e magWhoIAm(void);
+    uint8_t readMag(AK09916_Reg_Addr_e reg);
+    ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata);
 };
 
 // I2C
@@ -128,13 +136,6 @@ class ICM_20948_I2C : public ICM_20948
     ICM_20948_I2C(); // Constructor
 
     virtual ICM_20948_Status_e begin(TwoWire &wirePort = Wire, bool ad0val = true, uint8_t ad0pin = ICM_20948_ARD_UNUSED_PIN);
-    virtual ICM_20948_Status_e readMag(uint8_t reg, uint8_t *pdata, uint8_t len);
-    virtual ICM_20948_Status_e writeMag(uint8_t reg, uint8_t *pdata, uint8_t len);
-
-    ICM_20948_Status_e startupMagnetometer(void);
-    ICM_20948_Status_e magWhoIAm(void);
-    bool magIsConnected(void);
-    ICM_20948_Status_e getMagnetometerData(ICM_20948_AGMT_t *pagmt);
 };
 
 // SPI