diff --git a/examples/mpu6050/main.go b/examples/mpu6050/main.go index 90c224039..daab0053d 100644 --- a/examples/mpu6050/main.go +++ b/examples/mpu6050/main.go @@ -5,18 +5,32 @@ import ( "machine" "time" + "tinygo.org/x/drivers" "tinygo.org/x/drivers/mpu6050" ) func main() { machine.I2C0.Configure(machine.I2CConfig{}) - accel := mpu6050.New(machine.I2C0) - accel.Configure() + mpuDevice := mpu6050.New(machine.I2C0, mpu6050.DefaultAddress) + // Configure the device with default configuration. + err := mpuDevice.Configure(mpu6050.Config{}) + if err != nil { + panic(err.Error()) + } for { - x, y, z := accel.ReadAcceleration() - println(x, y, z) time.Sleep(time.Millisecond * 100) + err := mpuDevice.Update(drivers.AllMeasurements) + if err != nil { + println("error reading from mpu6050:", err.Error()) + continue + } + print("acceleration: ") + println(mpuDevice.Acceleration()) + print("angular velocity:") + println(mpuDevice.AngularVelocity()) + print("temperature celsius:") + println(mpuDevice.Temperature() / 1000) } } diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go index c6e945da2..a21c98a3a 100644 --- a/mpu6050/mpu6050.go +++ b/mpu6050/mpu6050.go @@ -6,90 +6,226 @@ // https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf package mpu6050 // import "tinygo.org/x/drivers/mpu6050" -import "tinygo.org/x/drivers" +import ( + "encoding/binary" + "errors" -// Device wraps an I2C connection to a MPU6050 device. + "tinygo.org/x/drivers" +) + +const DefaultAddress = 0x68 + +// RangeAccel defines the range of the accelerometer. +// Allowed values are 2, 4, 8 and 16 with the unit g (gravity). +type RangeAccel uint8 + +// RangeGyro defines the range of the gyroscope. +// Allowed values are 250, 500, 1000 and 2000 with the unit °/s (degree per second). +type RangeGyro uint8 + +var ( + errInvalidRangeAccel = errors.New("mpu6050: invalid range for accelerometer") + errInvalidRangeGyro = errors.New("mpu6050: invalid range for gyroscope") +) + +type Config struct { + // Use ACCEL_RANGE_2 through ACCEL_RANGE_16. + AccelRange RangeAccel + // Use GYRO_RANGE_250 through GYRO_RANGE_2000 + GyroRange RangeGyro + sampleRatio byte // TODO(soypat): expose these as configurable. + clkSel byte +} + +// Compile-time guarantee that Device implements the drivers.Sensor interface. +var _ drivers.Sensor = &Device{} + +// Device contains MPU board abstraction for usage type Device struct { - bus drivers.I2C - Address uint16 + conn drivers.I2C + aRange int32 //Gyroscope FSR acording to SetAccelRange input + gRange int32 //Gyroscope FSR acording to SetGyroRange input + // data contains the accelerometer, gyroscope and temperature data read + // in the last call via the Update method. The data is stored as seven 16bit unsigned + // integers in big endian format: + // + // | ax | ay | az | temp | gx | gy | gz | + data [14]byte + address byte } -// New creates a new MPU6050 connection. The I2C bus must already be -// configured. -// -// This function only creates the Device object, it does not touch the device. -func New(bus drivers.I2C) Device { - return Device{bus, Address} +// New instantiates and initializes a MPU6050 struct without writing/reading +// i2c bus. Typical I2C MPU6050 address is 0x68. +func New(bus drivers.I2C, addr uint16) *Device { + p := &Device{} + p.address = uint8(addr) + p.conn = bus + return p +} + +// Init configures the necessary registers for using the +// MPU6050. It sets the range of both the accelerometer +// and the gyroscope, the sample rate, the clock source +// and wakes up the peripheral. +func (p *Device) Configure(data Config) (err error) { + if err = p.Sleep(false); err != nil { + return err + } + if err = p.setClockSource(data.clkSel); err != nil { + return err + } + if err = p.setSampleRate(data.sampleRatio); err != nil { + return err + } + if err = p.setRangeGyro(data.GyroRange); err != nil { + return err + } + if err = p.setRangeAccel(data.AccelRange); err != nil { + return err + } + return nil } -// Connected returns whether a MPU6050 has been found. -// It does a "who am I" request and checks the response. func (d Device) Connected() bool { data := []byte{0} - d.bus.ReadRegister(uint8(d.Address), WHO_AM_I, data) + d.read(_WHO_AM_I, data) return data[0] == 0x68 } -// Configure sets up the device for communication. -func (d Device) Configure() error { - return d.SetClockSource(CLOCK_INTERNAL) -} - -// ReadAcceleration reads the current acceleration from the device and returns -// it in µg (micro-gravity). When one of the axes is pointing straight to Earth -// and the sensor is not moving the returned value will be around 1000000 or -// -1000000. -func (d Device) ReadAcceleration() (x int32, y int32, z int32) { - data := make([]byte, 6) - d.bus.ReadRegister(uint8(d.Address), ACCEL_XOUT_H, data) - // Now do two things: - // 1. merge the two values to a 16-bit number (and cast to a 32-bit integer) - // 2. scale the value to bring it in the -1000000..1000000 range. - // This is done with a trick. What we do here is essentially multiply by - // 1000000 and divide by 16384 to get the original scale, but to avoid - // overflow we do it at 1/64 of the value: - // 1000000 / 64 = 15625 - // 16384 / 64 = 256 - x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256 - y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256 - z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256 - return -} - -// ReadRotation reads the current rotation from the device and returns it in -// µ°/s (micro-degrees/sec). This means that if you were to do a complete +// Update fetches the latest data from the MPU6050 +func (p *Device) Update(which drivers.Measurement) (err error) { + // TODO(soypat): This could be optimized to read only the requested data. + // Should have benchmarks to show the difference. + const available = drivers.Acceleration | drivers.AngularVelocity | drivers.Temperature + if which&available == 0 { + return nil // No relevant data requested. + } + if err = p.read(_ACCEL_XOUT_H, p.data[:]); err != nil { + return err + } + return nil +} + +// Acceleration returns last read acceleration in µg (micro-gravity). +// When one of the axes is pointing straight to Earth and the sensor is not +// moving the returned value will be around 1000000 or -1000000. +func (d *Device) Acceleration() (ax, ay, az int32) { + const accelOffset = 0 + ax = int32(convertWord(d.data[accelOffset+0:])) * 15625 / 512 * d.aRange + ay = int32(convertWord(d.data[accelOffset+2:])) * 15625 / 512 * d.aRange + az = int32(convertWord(d.data[accelOffset+4:])) * 15625 / 512 * d.aRange + return ax, ay, az +} + +// AngularVelocity reads the current angular velocity from the device and returns it in +// µ°/rad (micro-radians/sec). This means that if you were to do a complete // rotation along one axis and while doing so integrate all values over time, -// you would get a value close to 360000000. -func (d Device) ReadRotation() (x int32, y int32, z int32) { - data := make([]byte, 6) - d.bus.ReadRegister(uint8(d.Address), GYRO_XOUT_H, data) - // First the value is converted from a pair of bytes to a signed 16-bit - // value and then to a signed 32-bit value to avoid integer overflow. - // Then the value is scaled to µ°/s (micro-degrees per second). - // This is done in the following steps: - // 1. Multiply by 250 * 1000_000 - // 2. Divide by 32768 - // The following calculation (x * 15625 / 2048 * 1000) is essentially the - // same but avoids overflow. First both operations are divided by 16 leading - // to multiply by 15625000 and divide by 2048, and then part of the multiply - // is done after the divide instead of before. - x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000 - y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000 - z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000 - return -} - -// SetClockSource allows the user to configure the clock source. -func (d Device) SetClockSource(source uint8) error { - return d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{source}) -} - -// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope. -func (d Device) SetFullScaleGyroRange(rng uint8) error { - return d.bus.WriteRegister(uint8(d.Address), GYRO_CONFIG, []uint8{rng}) -} - -// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer. -func (d Device) SetFullScaleAccelRange(rng uint8) error { - return d.bus.WriteRegister(uint8(d.Address), ACCEL_CONFIG, []uint8{rng}) +// you would get a value close to 6.3 radians (360 degrees). +func (d *Device) AngularVelocity() (gx, gy, gz int32) { + const angvelOffset = 8 + _ = d.data[angvelOffset+5] // This line fails to compile if RawData is too short. + gx = int32(convertWord(d.data[angvelOffset+0:])) * 4363 / 8192 * d.gRange + gy = int32(convertWord(d.data[angvelOffset+2:])) * 4363 / 8192 * d.gRange + gz = int32(convertWord(d.data[angvelOffset+4:])) * 4363 / 8192 * d.gRange + return gx, gy, gz +} + +// Temperature returns the temperature of the device in milli-centigrade. +func (d *Device) Temperature() (Celsius int32) { + const tempOffset = 6 + return 1506*int32(convertWord(d.data[tempOffset:]))/512 + 37*1000 +} + +func convertWord(buf []byte) int16 { + return int16(binary.BigEndian.Uint16(buf)) +} + +// setSampleRate sets the sample rate for the FIFO, +// register ouput and DMP. The sample rate is determined +// by: +// +// SR = Gyroscope Output Rate / (1 + srDiv) +// +// The Gyroscope Output Rate is 8kHz when the DLPF is +// disabled and 1kHz otherwise. The maximum sample rate +// for the accelerometer is 1kHz, if a higher sample rate +// is chosen, the same accelerometer sample will be output. +func (p *Device) setSampleRate(srDiv byte) (err error) { + // setSampleRate + var sr [1]byte + sr[0] = srDiv + if err = p.write8(_SMPRT_DIV, sr[0]); err != nil { + return err + } + return nil +} + +// setClockSource configures the source of the clock +// for the peripheral. +func (p *Device) setClockSource(clkSel byte) (err error) { + return p.writeMasked(_PWR_MGMT_1, _CLK_SEL_MASK, clkSel) +} + +// setRangeGyro configures the full scale range of the gyroscope. +// It has four possible values +- 250°/s, 500°/s, 1000°/s, 2000°/s. +func (p *Device) setRangeGyro(gyroRange RangeGyro) (err error) { + switch gyroRange { + case RangeGyro250: + p.gRange = 250 + case RangeGyro500: + p.gRange = 500 + case RangeGyro1000: + p.gRange = 1000 + case RangeGyro2000, rangeGyroDefault: + gyroRange = RangeGyro2000 + p.gRange = 2000 + default: + return errInvalidRangeGyro + } + return p.writeMasked(_GYRO_CONFIG, _G_FS_SEL, uint8(gyroRange-1)<<_G_FS_SHIFT) +} + +// setRangeAccel configures the full scale range of the accelerometer. +// It has four possible values +- 2g, 4g, 8g, 16g. +// The function takes values of accRange from 0-3 where 0 means the +// lowest FSR (2g) and 3 is the highest FSR (16g) +func (p *Device) setRangeAccel(accRange RangeAccel) (err error) { + switch accRange { + case RangeAccel2: + p.aRange = 2 + case RangeAccel4: + p.aRange = 4 + case RangeAccel8: + p.aRange = 8 + case RangeAccel16, rangeAccelDefault: + accRange = RangeAccel16 + p.aRange = 16 + default: + return errInvalidRangeAccel + } + return p.writeMasked(_ACCEL_CONFIG, _AFS_SEL, uint8(accRange-1)<<_AFS_SHIFT) +} + +// Sleep sets the sleep bit on the power managment 1 field. +// When the recieved bool is true, it sets the bit to 1 thus putting +// the peripheral in sleep mode. +// When false is recieved the bit is set to 0 and the peripheral wakes up. +func (p *Device) Sleep(sleepEnabled bool) (err error) { + return p.writeMasked(_PWR_MGMT_1, _SLEEP_MASK, b2u8(sleepEnabled)<<_SLEEP_SHIFT) +} + +func (d *Device) writeMasked(reg byte, mask byte, value byte) error { + var b [1]byte + if err := d.read(reg, b[:]); err != nil { + return err + } + b[0] = (b[0] &^ mask) | value&mask + return d.write8(reg, b[0]) +} + +func b2u8(b bool) byte { + if b { + return 1 + } + return 0 } diff --git a/mpu6050/registers.go b/mpu6050/registers.go index 4b74369f5..ca6c5e0ae 100644 --- a/mpu6050/registers.go +++ b/mpu6050/registers.go @@ -1,132 +1,188 @@ package mpu6050 -// Constants/addresses used for I2C. +// read reads register reg and writes n bytes to b where +// n is the length of b. +func (p *Device) read(reg uint8, buff []byte) error { + buf := [1]byte{reg} + return p.conn.Tx(uint16(p.address), buf[:1], buff) +} -// The I2C address which this device listens to. -const Address = 0x68 +// write8 writes a registry byte +func (p *Device) write8(reg uint8, datum byte) error { + var buff [2]byte + buff[0] = reg + buff[1] = datum + return p.conn.Tx(uint16(p.address), buff[:], nil) +} + +// MPU 6050 REGISTER ADRESSES +const ( + _SMPRT_DIV uint8 = 0x19 + _CONFIG uint8 = 0x1A + _GYRO_CONFIG uint8 = 0x1B + _ACCEL_CONFIG uint8 = 0x1C + _FIFO_EN uint8 = 0x23 +) + +// MPU 6050 MASKS +const ( + _G_FS_SEL uint8 = 0x18 + _AFS_SEL uint8 = 0x18 + _CLK_SEL_MASK uint8 = 0x07 + _SLEEP_MASK uint8 = 0x40 +) + +// MPU 6050 SHIFTS +const ( + _AFS_SHIFT uint8 = 3 + _G_FS_SHIFT uint8 = 3 + _SLEEP_SHIFT uint8 = 6 +) + +// Gyroscope ranges for Init configuration +const ( + rangeGyroDefault = iota + // 250°/s + RangeGyro250 + // 500°/s + RangeGyro500 + // 1000°/s + RangeGyro1000 + // 2000°/s + RangeGyro2000 +) + +// Accelerometer ranges for Init configuration +const ( + rangeAccelDefault RangeAccel = iota + // 2g + RangeAccel2 + // 4g + RangeAccel4 + // 8g + RangeAccel8 + // 16g + RangeAccel16 +) // Registers. Names, addresses and comments copied from the datasheet. const ( // Self test registers - SELF_TEST_X = 0x0D - SELF_TEST_Y = 0x0E - SELF_TEST_Z = 0x0F - SELF_TEST_A = 0x10 + _SELF_TEST_X = 0x0D + _SELF_TEST_Y = 0x0E + _SELF_TEST_Z = 0x0F + _SELF_TEST_A = 0x10 - SMPLRT_DIV = 0x19 // Sample rate divider - CONFIG = 0x1A // Configuration - GYRO_CONFIG = 0x1B // Gyroscope configuration - ACCEL_CONFIG = 0x1C // Accelerometer configuration - FIFO_EN = 0x23 // FIFO enable + _SMPLRT_DIV = 0x19 // Sample rate divider // I2C pass-through configuration - I2C_MST_CTRL = 0x24 - I2C_SLV0_ADDR = 0x25 - I2C_SLV0_REG = 0x26 - I2C_SLV0_CTRL = 0x27 - I2C_SLV1_ADDR = 0x28 - I2C_SLV1_REG = 0x29 - I2C_SLV1_CTRL = 0x2A - I2C_SLV2_ADDR = 0x2B - I2C_SLV2_REG = 0x2C - I2C_SLV2_CTRL = 0x2D - I2C_SLV3_ADDR = 0x2E - I2C_SLV3_REG = 0x2F - I2C_SLV3_CTRL = 0x30 - I2C_SLV4_ADDR = 0x31 - I2C_SLV4_REG = 0x32 - I2C_SLV4_DO = 0x33 - I2C_SLV4_CTRL = 0x34 - I2C_SLV4_DI = 0x35 - I2C_MST_STATUS = 0x36 + _I2C_MST_CTRL = 0x24 + _I2C_SLV0_ADDR = 0x25 + _I2C_SLV0_REG = 0x26 + _I2C_SLV0_CTRL = 0x27 + _I2C_SLV1_ADDR = 0x28 + _I2C_SLV1_REG = 0x29 + _I2C_SLV1_CTRL = 0x2A + _I2C_SLV2_ADDR = 0x2B + _I2C_SLV2_REG = 0x2C + _I2C_SLV2_CTRL = 0x2D + _I2C_SLV3_ADDR = 0x2E + _I2C_SLV3_REG = 0x2F + _I2C_SLV3_CTRL = 0x30 + _I2C_SLV4_ADDR = 0x31 + _I2C_SLV4_REG = 0x32 + _I2C_SLV4_DO = 0x33 + _I2C_SLV4_CTRL = 0x34 + _I2C_SLV4_DI = 0x35 + _I2C_MST_STATUS = 0x36 // Interrupt configuration - INT_PIN_CFG = 0x37 // Interrupt pin/bypass enable configuration - INT_ENABLE = 0x38 // Interrupt enable - INT_STATUS = 0x3A // Interrupt status + _INT_PIN_CFG = 0x37 // Interrupt pin/bypass enable configuration + _INT_ENABLE = 0x38 // Interrupt enable + _INT_STATUS = 0x3A // Interrupt status // Accelerometer measurements - ACCEL_XOUT_H = 0x3B - ACCEL_XOUT_L = 0x3C - ACCEL_YOUT_H = 0x3D - ACCEL_YOUT_L = 0x3E - ACCEL_ZOUT_H = 0x3F - ACCEL_ZOUT_L = 0x40 + _ACCEL_XOUT_H = 0x3B + _ACCEL_XOUT_L = 0x3C + _ACCEL_YOUT_H = 0x3D + _ACCEL_YOUT_L = 0x3E + _ACCEL_ZOUT_H = 0x3F + _ACCEL_ZOUT_L = 0x40 // Temperature measurement - TEMP_OUT_H = 0x41 - TEMP_OUT_L = 0x42 + _TEMP_OUT_H = 0x41 + _TEMP_OUT_L = 0x42 // Gyroscope measurements - GYRO_XOUT_H = 0x43 - GYRO_XOUT_L = 0x44 - GYRO_YOUT_H = 0x45 - GYRO_YOUT_L = 0x46 - GYRO_ZOUT_H = 0x47 - GYRO_ZOUT_L = 0x48 + _GYRO_XOUT_H = 0x43 + _GYRO_XOUT_L = 0x44 + _GYRO_YOUT_H = 0x45 + _GYRO_YOUT_L = 0x46 + _GYRO_ZOUT_H = 0x47 + _GYRO_ZOUT_L = 0x48 // External sensor data - EXT_SENS_DATA_00 = 0x49 - EXT_SENS_DATA_01 = 0x4A - EXT_SENS_DATA_02 = 0x4B - EXT_SENS_DATA_03 = 0x4C - EXT_SENS_DATA_04 = 0x4D - EXT_SENS_DATA_05 = 0x4E - EXT_SENS_DATA_06 = 0x4F - EXT_SENS_DATA_07 = 0x50 - EXT_SENS_DATA_08 = 0x51 - EXT_SENS_DATA_09 = 0x52 - EXT_SENS_DATA_10 = 0x53 - EXT_SENS_DATA_11 = 0x54 - EXT_SENS_DATA_12 = 0x55 - EXT_SENS_DATA_13 = 0x56 - EXT_SENS_DATA_14 = 0x57 - EXT_SENS_DATA_15 = 0x58 - EXT_SENS_DATA_16 = 0x59 - EXT_SENS_DATA_17 = 0x5A - EXT_SENS_DATA_18 = 0x5B - EXT_SENS_DATA_19 = 0x5C - EXT_SENS_DATA_20 = 0x5D - EXT_SENS_DATA_21 = 0x5E - EXT_SENS_DATA_22 = 0x5F - EXT_SENS_DATA_23 = 0x60 + _EXT_SENS_DATA_00 = 0x49 + _EXT_SENS_DATA_01 = 0x4A + _EXT_SENS_DATA_02 = 0x4B + _EXT_SENS_DATA_03 = 0x4C + _EXT_SENS_DATA_04 = 0x4D + _EXT_SENS_DATA_05 = 0x4E + _EXT_SENS_DATA_06 = 0x4F + _EXT_SENS_DATA_07 = 0x50 + _EXT_SENS_DATA_08 = 0x51 + _EXT_SENS_DATA_09 = 0x52 + _EXT_SENS_DATA_10 = 0x53 + _EXT_SENS_DATA_11 = 0x54 + _EXT_SENS_DATA_12 = 0x55 + _EXT_SENS_DATA_13 = 0x56 + _EXT_SENS_DATA_14 = 0x57 + _EXT_SENS_DATA_15 = 0x58 + _EXT_SENS_DATA_16 = 0x59 + _EXT_SENS_DATA_17 = 0x5A + _EXT_SENS_DATA_18 = 0x5B + _EXT_SENS_DATA_19 = 0x5C + _EXT_SENS_DATA_20 = 0x5D + _EXT_SENS_DATA_21 = 0x5E + _EXT_SENS_DATA_22 = 0x5F + _EXT_SENS_DATA_23 = 0x60 // I2C peripheral data out - I2C_PER0_DO = 0x63 - I2C_PER1_DO = 0x64 - I2C_PER2_DO = 0x65 - I2C_PER3_DO = 0x66 - I2C_MST_DELAY_CT = 0x67 + _I2C_PER0_DO = 0x63 + _I2C_PER1_DO = 0x64 + _I2C_PER2_DO = 0x65 + _I2C_PER3_DO = 0x66 + _I2C_MST_DELAY_CT = 0x67 // Clock settings - CLOCK_INTERNAL = 0x00 - CLOCK_PLL_XGYRO = 0x01 - CLOCK_PLL_YGYRO = 0x02 - CLOCK_PLL_ZGYRO = 0x03 - CLOCK_PLL_EXTERNAL_32_768_KZ = 0x04 - CLOCK_PLL_EXTERNAL_19_2_MHZ = 0x05 - CLOCK_RESERVED = 0x06 - CLOCK_STOP = 0x07 + _CLOCK_INTERNAL = 0x00 + _CLOCK_PLL_XGYRO = 0x01 + _CLOCK_PLL_YGYRO = 0x02 + _CLOCK_PLL_ZGYRO = 0x03 + _CLOCK_PLL_EXTERNAL_32_768_KZ = 0x04 + _CLOCK_PLL_EXTERNAL_19_2_MHZ = 0x05 + _CLOCK_RESERVED = 0x06 + _CLOCK_STOP = 0x07 // Accelerometer settings - AFS_RANGE_2G = 0x00 - AFS_RANGE_4G = 0x01 - AFS_RANGE_8G = 0x02 - AFS_RANGE_16G = 0x03 + _AFS_RANGE_2G = 0x00 + _AFS_RANGE_4G = 0x01 + _AFS_RANGE_8G = 0x02 + _AFS_RANGE_16G = 0x03 // Gyroscope settings - FS_RANGE_250 = 0x00 - FS_RANGE_500 = 0x01 - FS_RANGE_1000 = 0x02 - FS_RANGE_2000 = 0x03 + _FS_RANGE_250 = 0x00 + _FS_RANGE_500 = 0x01 + _FS_RANGE_1000 = 0x02 + _FS_RANGE_2000 = 0x03 // other registers - SIGNAL_PATH_RES = 0x68 // Signal path reset - USER_CTRL = 0x6A // User control - PWR_MGMT_1 = 0x6B // Power Management 1 - PWR_MGMT_2 = 0x6C // Power Management 2 - FIFO_COUNTH = 0x72 // FIFO count registers (high bits) - FIFO_COUNTL = 0x73 // FIFO count registers (low bits) - FIFO_R_W = 0x74 // FIFO read/write - WHO_AM_I = 0x75 // Who am I + _SIGNAL_PATH_RES = 0x68 // Signal path reset + _USER_CTRL = 0x6A // User control + _PWR_MGMT_1 = 0x6B // Power Management 1 + _PWR_MGMT_2 = 0x6C // Power Management 2 + _FIFO_COUNTH = 0x72 // FIFO count registers (high bits) + _FIFO_COUNTL = 0x73 // FIFO count registers (low bits) + _FIFO_R_W = 0x74 // FIFO read/write + _WHO_AM_I = 0x75 // Who am I register )