File icm20948.h¶
File List > dev > icm20948.h
Go to the documentation of this file
Source Code¶
#pragma once
#ifndef DSY_ICM20948_H
#define DSY_ICM20948_H
// Misc configuration macros
#define I2C_MASTER_RESETS_BEFORE_FAIL \
5
#define NUM_FINISHED_CHECKS \
100
// Bank 0
#define ICM20X_B0_WHOAMI 0x00
#define ICM20X_B0_USER_CTRL 0x03
#define ICM20X_B0_LP_CONFIG 0x05
#define ICM20X_B0_REG_INT_PIN_CFG 0xF
#define ICM20X_B0_REG_INT_ENABLE 0x10
#define ICM20X_B0_REG_INT_ENABLE_1 0x11
#define ICM20X_B0_I2C_MST_STATUS \
0x17
#define ICM20X_B0_REG_BANK_SEL 0x7F
#define ICM20X_B0_PWR_MGMT_1 0x06
#define ICM20X_B0_ACCEL_XOUT_H 0x2D
#define ICM20X_B0_GYRO_XOUT_H 0x33
// Bank 2
#define ICM20X_B2_GYRO_SMPLRT_DIV 0x00
#define ICM20X_B2_GYRO_CONFIG_1 0x01
#define ICM20X_B2_ACCEL_SMPLRT_DIV_1 0x10
#define ICM20X_B2_ACCEL_SMPLRT_DIV_2 0x11
#define ICM20X_B2_ACCEL_CONFIG_1 0x14
// Bank 3
#define ICM20X_B3_I2C_MST_ODR_CONFIG 0x0
#define ICM20X_B3_I2C_MST_CTRL 0x1
#define ICM20X_B3_I2C_MST_DELAY_CTRL 0x2
#define ICM20X_B3_I2C_SLV0_ADDR \
0x3
#define ICM20X_B3_I2C_SLV0_REG \
0x4
#define ICM20X_B3_I2C_SLV0_CTRL 0x5
#define ICM20X_B3_I2C_SLV0_DO 0x6
#define ICM20X_B3_I2C_SLV4_ADDR \
0x13
#define ICM20X_B3_I2C_SLV4_REG \
0x14
#define ICM20X_B3_I2C_SLV4_CTRL 0x15
#define ICM20X_B3_I2C_SLV4_DO 0x16
#define ICM20X_B3_I2C_SLV4_DI 0x17
#define ICM20948_CHIP_ID 0xEA
#define ICM20948_I2CADDR_DEFAULT 0x69
#define ICM20948_MAG_ID 0x09
#define ICM20948_UT_PER_LSB 0.15
#define AK09916_WIA2 0x01
#define AK09916_ST1 0x10
#define AK09916_HXL 0x11
#define AK09916_HXH 0x12
#define AK09916_HYL 0x13
#define AK09916_HYH 0x14
#define AK09916_HZL 0x15
#define AK09916_HZH 0x16
#define AK09916_ST2 0x18
#define AK09916_CNTL2 0x31
#define AK09916_CNTL3 0x32
#define SENSORS_GRAVITY_EARTH (9.80665F)
#define SENSORS_DPS_TO_RADS (0.017453293F)
namespace daisy
{
class Icm20948I2CTransport
{
public:
Icm20948I2CTransport() {}
~Icm20948I2CTransport() {}
struct Config
{
I2CHandle::Config::Peripheral periph;
I2CHandle::Config::Speed speed;
Pin scl;
Pin sda;
uint8_t address;
Config()
{
address = ICM20948_I2CADDR_DEFAULT;
periph = I2CHandle::Config::Peripheral::I2C_1;
speed = I2CHandle::Config::Speed::I2C_400KHZ;
scl = Pin(PORTB, 8);
sda = Pin(PORTB, 9);
}
};
inline void Init(Config config)
{
config_ = config;
I2CHandle::Config i2c_config;
i2c_config.mode = I2CHandle::Config::Mode::I2C_MASTER;
i2c_config.periph = config.periph;
i2c_config.speed = config.speed;
i2c_config.pin_config.scl = config.scl;
i2c_config.pin_config.sda = config.sda;
i2c_.Init(i2c_config);
}
void Write(uint8_t *data, uint16_t size)
{
error_ |= I2CHandle::Result::OK
!= i2c_.TransmitBlocking(config_.address, data, size, 10);
}
void Read(uint8_t *data, uint16_t size)
{
error_ |= I2CHandle::Result::OK
!= i2c_.ReceiveBlocking(config_.address, data, size, 10);
}
void Write8(uint8_t reg, uint8_t value)
{
uint8_t buffer[2];
buffer[0] = reg;
buffer[1] = value;
Write(buffer, 2);
}
void Write16(uint8_t reg, uint16_t value)
{
uint8_t buffer[3];
buffer[0] = reg;
buffer[1] = value >> 8;
buffer[1] = value & 0xFF;
Write(buffer, 2);
}
void ReadReg(uint8_t reg, uint8_t *buff, uint8_t size)
{
Write(®, 1);
Read(buff, size);
}
uint8_t Read8(uint8_t reg)
{
uint8_t buffer;
ReadReg(reg, &buffer, 1);
return buffer;
}
bool GetError()
{
bool tmp = error_;
error_ = false;
return tmp;
}
private:
I2CHandle i2c_;
Config config_;
// true if error has occured since last check
bool error_;
};
class Icm20948SpiTransport
{
public:
Icm20948SpiTransport() {}
~Icm20948SpiTransport() {}
struct Config
{
SpiHandle::Config::Peripheral periph;
Pin sclk;
Pin miso;
Pin mosi;
Pin nss;
Config()
{
periph = SpiHandle::Config::Peripheral::SPI_1;
sclk = Pin(PORTG, 11);
miso = Pin(PORTB, 4);
mosi = Pin(PORTB, 5);
nss = Pin(PORTG, 10);
}
};
inline void Init(Config config)
{
SpiHandle::Config spi_conf;
spi_conf.mode = SpiHandle::Config::Mode::MASTER;
spi_conf.direction = SpiHandle::Config::Direction::TWO_LINES;
spi_conf.clock_polarity = SpiHandle::Config::ClockPolarity::LOW;
spi_conf.clock_phase = SpiHandle::Config::ClockPhase::ONE_EDGE;
spi_conf.baud_prescaler = SpiHandle::Config::BaudPrescaler::PS_2;
spi_conf.nss = SpiHandle::Config::NSS::SOFT;
spi_conf.periph = config.periph;
spi_conf.pin_config.sclk = config.sclk;
spi_conf.pin_config.miso = config.miso;
spi_conf.pin_config.mosi = config.mosi;
spi_conf.pin_config.nss = config.nss;
spi_.Init(spi_conf);
}
void Write(uint8_t *data, uint16_t size)
{
error_ |= SpiHandle::Result::OK != spi_.BlockingTransmit(data, size);
}
void Read(uint8_t *data, uint16_t size)
{
error_ |= SpiHandle::Result::OK != spi_.BlockingReceive(data, size, 10);
}
void Write8(uint8_t reg, uint8_t value)
{
uint8_t buffer[2];
buffer[0] = reg & ~0x80;
buffer[1] = value;
Write(buffer, 2);
}
void Write16(uint8_t reg, uint16_t value)
{
uint8_t buffer[3];
buffer[0] = reg & ~0x80;
buffer[1] = value >> 8;
buffer[2] = value & 0xFF;
Write(buffer, 3);
}
void ReadReg(uint8_t reg, uint8_t *buff, uint8_t size)
{
reg = uint8_t(reg | 0x80);
Write(®, 1);
Read(buff, size);
}
uint8_t Read8(uint8_t reg)
{
uint8_t buffer;
ReadReg(reg, &buffer, 1);
return buffer;
}
bool GetError()
{
bool tmp = error_;
error_ = false;
return tmp;
}
private:
SpiHandle spi_;
bool error_;
};
template <typename Transport>
class Icm20948
{
public:
Icm20948() {}
~Icm20948() {}
struct Config
{
typename Transport::Config transport_config;
Config() {}
};
struct Icm20948Vect
{
float x;
float y;
float z;
};
enum icm20948_accel_range_t
{
ICM20948_ACCEL_RANGE_2_G,
ICM20948_ACCEL_RANGE_4_G,
ICM20948_ACCEL_RANGE_8_G,
ICM20948_ACCEL_RANGE_16_G,
};
enum icm20948_gyro_range_t
{
ICM20948_GYRO_RANGE_250_DPS,
ICM20948_GYRO_RANGE_500_DPS,
ICM20948_GYRO_RANGE_1000_DPS,
ICM20948_GYRO_RANGE_2000_DPS,
};
enum ak09916_data_rate_t
{
AK09916_MAG_DATARATE_SHUTDOWN = 0x0,
AK09916_MAG_DATARATE_SINGLE
= 0x1,
AK09916_MAG_DATARATE_10_HZ = 0x2,
AK09916_MAG_DATARATE_20_HZ = 0x4,
AK09916_MAG_DATARATE_50_HZ = 0x6,
AK09916_MAG_DATARATE_100_HZ = 0x8,
};
enum Result
{
OK = 0,
ERR
};
Result Init(Config config)
{
config_ = config;
transport_.Init(config_.transport_config);
SetBank(0);
uint8_t chip_id = Read8(ICM20X_B0_WHOAMI);
if(chip_id != ICM20948_CHIP_ID)
{
return ERR;
}
_sensorid_accel = 0;
_sensorid_gyro = 1;
_sensorid_mag = 2;
_sensorid_temp = 3;
Reset();
// take out of default sleep state
WriteBits(ICM20X_B0_PWR_MGMT_1, 0, 1, 6);
// 3 will be the largest range for either sensor
WriteGyroRange(3);
WriteAccelRange(3);
// 1100Hz/(1+10) = 100Hz
SetGyroRateDivisor(10);
// # 1125Hz/(1+20) = 53.57Hz
SetAccelRateDivisor(20);
System::Delay(20);
return GetTransportError();
}
void Reset()
{
SetBank(0);
WriteBits(ICM20X_B0_PWR_MGMT_1, 1, 1, 7);
System::Delay(20);
while(ReadBits(ICM20X_B0_PWR_MGMT_1, 1, 7))
{
System::Delay(10);
};
System::Delay(50);
}
uint8_t GetMagId()
{
// verify the magnetometer id
return ReadExternalRegister(0x8C, 0x01);
}
Result SetupMag()
{
SetI2CBypass(false);
ConfigureI2CMaster();
EnableI2CMaster(true);
if(AuxI2CBusSetupFailed() == ERR)
{
return ERR;
}
// set mag data rate
if(!SetMagDataRate(AK09916_MAG_DATARATE_100_HZ))
{
// Serial.println("Error setting magnetometer data rate on external bus");
return ERR;
}
// TODO: extract method
// Set up Slave0 to proxy Mag readings
SetBank(3);
// set up slave0 to proxy reads to mag
Write8(ICM20X_B3_I2C_SLV0_ADDR, 0x8C);
if(GetTransportError() != OK)
{
return ERR;
}
Write8(ICM20X_B3_I2C_SLV0_REG, 0x10);
if(GetTransportError() != OK)
{
return ERR;
}
// enable, read 9 bytes
Write8(ICM20X_B3_I2C_SLV0_CTRL, 0x89);
if(GetTransportError() != OK)
{
return ERR;
}
return OK;
}
uint8_t ReadMagRegister(uint8_t mag_reg_addr)
{
return ReadExternalRegister(0x8C, mag_reg_addr);
}
bool WriteMagRegister(uint8_t mag_reg_addr, uint8_t value)
{
return WriteExternalRegister(0x0C, mag_reg_addr, value);
}
void ScaleValues()
{
icm20948_gyro_range_t gyro_range
= (icm20948_gyro_range_t)current_gyro_range_;
icm20948_accel_range_t accel_range
= (icm20948_accel_range_t)current_accel_range_;
float accel_scale = 1.0;
float gyro_scale = 1.0;
if(gyro_range == ICM20948_GYRO_RANGE_250_DPS)
gyro_scale = 131.0;
if(gyro_range == ICM20948_GYRO_RANGE_500_DPS)
gyro_scale = 65.5;
if(gyro_range == ICM20948_GYRO_RANGE_1000_DPS)
gyro_scale = 32.8;
if(gyro_range == ICM20948_GYRO_RANGE_2000_DPS)
gyro_scale = 16.4;
if(accel_range == ICM20948_ACCEL_RANGE_2_G)
accel_scale = 16384.0;
if(accel_range == ICM20948_ACCEL_RANGE_4_G)
accel_scale = 8192.0;
if(accel_range == ICM20948_ACCEL_RANGE_8_G)
accel_scale = 4096.0;
if(accel_range == ICM20948_ACCEL_RANGE_16_G)
accel_scale = 2048.0;
gyroX = rawGyroX / gyro_scale;
gyroY = rawGyroY / gyro_scale;
gyroZ = rawGyroZ / gyro_scale;
accX = rawAccX / accel_scale;
accY = rawAccY / accel_scale;
accZ = rawAccZ / accel_scale;
magX = rawMagX * ICM20948_UT_PER_LSB;
magY = rawMagY * ICM20948_UT_PER_LSB;
magZ = rawMagZ * ICM20948_UT_PER_LSB;
}
void SetAccelRateDivisor(uint16_t new_accel_divisor)
{
SetBank(2);
Write16(ICM20X_B2_ACCEL_SMPLRT_DIV_1, new_accel_divisor);
SetBank(0);
}
icm20948_accel_range_t GetAccelRange()
{
return (icm20948_accel_range_t)ReadAccelRange();
}
uint8_t ReadAccelRange()
{
SetBank(2);
uint8_t range = ReadBits(ICM20X_B2_ACCEL_CONFIG_1, 2, 1);
SetBank(0);
return range;
}
void WriteAccelRange(uint8_t new_accel_range)
{
SetBank(2);
WriteBits(ICM20X_B2_ACCEL_CONFIG_1, new_accel_range, 2, 1);
current_accel_range_ = new_accel_range;
SetBank(0);
}
void SetAccelRange(icm20948_accel_range_t new_accel_range)
{
WriteAccelRange((uint8_t)new_accel_range);
}
void SetGyroRateDivisor(uint8_t new_gyro_divisor)
{
SetBank(2);
Write8(ICM20X_B2_GYRO_SMPLRT_DIV, new_gyro_divisor);
SetBank(0);
}
icm20948_gyro_range_t GetGyroRange()
{
return (icm20948_gyro_range_t)ReadGyroRange();
}
void SetGyroRange(icm20948_gyro_range_t new_gyro_range)
{
WriteGyroRange((uint8_t)new_gyro_range);
}
void WriteGyroRange(uint8_t new_gyro_range)
{
SetBank(2);
WriteBits(ICM20X_B2_GYRO_CONFIG_1, new_gyro_range, 2, 1);
current_gyro_range_ = new_gyro_range;
SetBank(0);
}
uint8_t ReadGyroRange()
{
SetBank(2);
uint8_t range = ReadBits(ICM20X_B2_GYRO_CONFIG_1, 2, 1);
SetBank(0);
return range;
}
ak09916_data_rate_t GetMagDataRate()
{
uint8_t raw_mag_rate = ReadMagRegister(AK09916_CNTL2);
return (ak09916_data_rate_t)(raw_mag_rate);
}
bool SetMagDataRate(ak09916_data_rate_t rate)
{
/* Following the datasheet, the sensor will be set to
* AK09916_MAG_DATARATE_SHUTDOWN followed by a 100ms delay, followed by
* setting the new data rate.
*
* See page 9 of https://www.y-ic.es/datasheet/78/SMDSW.020-2OZ.pdf */
// don't need to read/mask because there's nothing else in the register and
// it's right justified
bool success
= WriteMagRegister(AK09916_CNTL2, AK09916_MAG_DATARATE_SHUTDOWN);
System::Delay(1);
return WriteMagRegister(AK09916_CNTL2, rate) && success;
}
void SetBank(uint8_t bank_number)
{
Write8(ICM20X_B0_REG_BANK_SEL, (bank_number & 0b11) << 4);
}
uint8_t ReadExternalRegister(uint8_t slv_addr, uint8_t reg_addr)
{
return AuxillaryRegisterTransaction(true, slv_addr, reg_addr);
}
bool
WriteExternalRegister(uint8_t slv_addr, uint8_t reg_addr, uint8_t value)
{
return (bool)AuxillaryRegisterTransaction(
false, slv_addr, reg_addr, value);
}
uint8_t AuxillaryRegisterTransaction(bool read,
uint8_t slv_addr,
uint8_t reg_addr,
uint8_t value)
{
SetBank(3);
if(read)
{
// set high bit for read, presumably for multi-byte reads
slv_addr |= 0x80;
}
else
{
Write8(ICM20X_B3_I2C_SLV4_DO, value);
if(GetTransportError() == ERR)
{
return (uint8_t) false;
}
}
Write8(ICM20X_B3_I2C_SLV4_ADDR, slv_addr);
if(GetTransportError() == ERR)
{
return (uint8_t) false;
}
Write8(ICM20X_B3_I2C_SLV4_REG, reg_addr);
if(GetTransportError() == ERR)
{
return (uint8_t) false;
}
Write8(ICM20X_B3_I2C_SLV4_CTRL, 0x80);
if(GetTransportError() == ERR)
{
return (uint8_t) false;
}
SetBank(0);
uint8_t tries = 0;
// wait until the operation is finished
while(ReadBits(ICM20X_B0_I2C_MST_STATUS, 1, 6) != true)
{
tries++;
if(tries >= NUM_FINISHED_CHECKS)
{
return (uint8_t) false;
}
}
if(read)
{
SetBank(3);
return Read8(ICM20X_B3_I2C_SLV4_DI);
}
return (uint8_t) true;
}
void Process()
{
SetBank(0);
// reading 9 bytes of mag data to fetch the register that tells the mag we've
// read all the data
const uint8_t numbytes
= 14 + 9; // Read Accel, gyro, temp, and 9 bytes of mag
uint8_t buffer[numbytes];
transport_.ReadReg(ICM20X_B0_ACCEL_XOUT_H, buffer, numbytes);
rawAccX = buffer[0] << 8 | buffer[1];
rawAccY = buffer[2] << 8 | buffer[3];
rawAccZ = buffer[4] << 8 | buffer[5];
rawGyroX = buffer[6] << 8 | buffer[7];
rawGyroY = buffer[8] << 8 | buffer[9];
rawGyroZ = buffer[10] << 8 | buffer[11];
temperature = buffer[12] << 8 | buffer[13];
rawMagX = ((buffer[16] << 8)
| (buffer[15] & 0xFF)); // Mag data is read little endian
rawMagY = ((buffer[18] << 8) | (buffer[17] & 0xFF));
rawMagZ = ((buffer[20] << 8) | (buffer[19] & 0xFF));
ScaleValues();
SetBank(0);
}
Icm20948Vect GetAccelVect()
{
Icm20948Vect vect;
vect.x = accX * SENSORS_GRAVITY_EARTH;
vect.y = accY * SENSORS_GRAVITY_EARTH;
vect.z = accZ * SENSORS_GRAVITY_EARTH;
return vect;
}
Icm20948Vect GetGyroVect()
{
Icm20948Vect vect;
vect.x = gyroX * SENSORS_DPS_TO_RADS;
vect.y = gyroY * SENSORS_DPS_TO_RADS;
vect.z = gyroZ * SENSORS_DPS_TO_RADS;
return vect;
}
Icm20948Vect GetMagVect()
{
Icm20948Vect vect;
vect.x = magX;
vect.y = magY;
vect.z = magZ;
return vect;
}
float GetTemp() { return (temperature / 333.87) + 21.0; }
uint8_t Read8(uint8_t reg) { return transport_.Read8(reg); }
void Write8(uint8_t reg, uint8_t value)
{
return transport_.Write8(reg, value);
}
void Write16(uint8_t reg, uint16_t value)
{
return transport_.Write16(reg, value);
}
void ReadReg(uint8_t reg, uint8_t *buff, uint8_t size)
{
return transport_.ReadReg(reg, buff, size);
}
uint8_t ReadBits(uint8_t reg, uint8_t bits, uint8_t shift)
{
uint8_t val = Read8(reg);
val >>= shift;
return val & ((1 << (bits)) - 1);
}
void WriteBits(uint8_t reg, uint8_t data, uint8_t bits, uint8_t shift)
{
uint8_t val = Read8(reg);
// mask off the data before writing
uint8_t mask = (1 << (bits)) - 1;
data &= mask;
mask <<= shift;
val &= ~mask; // remove the current data at that spot
val |= data << shift; // and add in the new data
Write8(reg, val);
}
void SetI2CBypass(bool bypass_i2c)
{
SetBank(0);
WriteBits(ICM20X_B0_REG_INT_PIN_CFG, bypass_i2c, 1, 1);
}
Result EnableI2CMaster(bool enable_i2c_master)
{
SetBank(0);
WriteBits(ICM20X_B0_USER_CTRL, enable_i2c_master, 1, 5);
return GetTransportError();
}
Result ConfigureI2CMaster(void)
{
SetBank(3);
Write8(ICM20X_B3_I2C_MST_CTRL, 0x17);
return GetTransportError();
}
void ResetI2CMaster(void)
{
SetBank(0);
WriteBits(ICM20X_B0_USER_CTRL, true, 1, 1);
while(ReadBits(ICM20X_B0_USER_CTRL, 1, 1))
{
System::Delay(10);
}
System::Delay(100);
}
// A million thanks to the SparkFun folks for their library that I pillaged to
// write this method! See their Arduino library here:
// https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary
Result AuxI2CBusSetupFailed(void)
{
// check aux I2C bus connection by reading the magnetometer chip ID
for(int i = 0; i < I2C_MASTER_RESETS_BEFORE_FAIL; i++)
{
if(GetMagId() != ICM20948_MAG_ID)
{
ResetI2CMaster();
}
else
{
return ERR;
}
}
return OK;
}
Result GetTransportError() { return transport_.GetError() ? ERR : OK; }
private:
Config config_;
Transport transport_;
uint16_t _sensorid_accel,
_sensorid_gyro,
_sensorid_mag,
_sensorid_temp;
uint8_t current_accel_range_;
uint8_t current_gyro_range_;
int16_t rawAccX,
rawAccY,
rawAccZ,
rawTemp,
rawGyroX,
rawGyroY,
rawGyroZ,
rawMagX,
rawMagY,
rawMagZ;
float temperature,
accX,
accY,
accZ,
gyroX,
gyroY,
gyroZ,
magX,
magY,
magZ;
};
using Icm20948I2C = Icm20948<Icm20948I2CTransport>;
using Icm20948Spi = Icm20948<Icm20948SpiTransport>;
} // namespace daisy
#endif