From f3f2d65878c58912acb1c1018a93a1524d817631 Mon Sep 17 00:00:00 2001 From: ivoszz Date: Wed, 10 May 2023 16:27:35 +0200 Subject: [PATCH] mpu6886: initial implementation --- examples/mpu6886/main.go | 22 +++++ mpu6886/mpu6886.go | 192 +++++++++++++++++++++++++++++++++++++++ mpu6886/registers.go | 116 +++++++++++++++++++++++ 3 files changed, 330 insertions(+) create mode 100644 examples/mpu6886/main.go create mode 100644 mpu6886/mpu6886.go create mode 100644 mpu6886/registers.go diff --git a/examples/mpu6886/main.go b/examples/mpu6886/main.go new file mode 100644 index 0000000..b369a9a --- /dev/null +++ b/examples/mpu6886/main.go @@ -0,0 +1,22 @@ +// Connects to an MPU6886 I2C accelerometer/gyroscope. +package main + +import ( + "machine" + "time" + + "tinygo.org/x/drivers/mpu6886" +) + +func main() { + machine.I2C0.Configure(machine.I2CConfig{}) + + accel := mpu6886.New(machine.I2C0) + accel.Configure(mpu6886.Config{}) + + for { + x, y, z, _ := accel.ReadAcceleration() + println(x, y, z) + time.Sleep(time.Millisecond * 100) + } +} diff --git a/mpu6886/mpu6886.go b/mpu6886/mpu6886.go new file mode 100644 index 0000000..6bbe84e --- /dev/null +++ b/mpu6886/mpu6886.go @@ -0,0 +1,192 @@ +// Package mpu6886 provides a driver for the MPU6886 accelerometer and gyroscope +// made by InvenSense. +// +// Datasheet: +// https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf +package mpu6886 // import "tinygo.org/x/drivers/mpu6886" + +import ( + "errors" + "time" + + "tinygo.org/x/drivers" +) + +const WhoAmI = 0x19 + +var errNotConnected = errors.New("mpu6886: failed to communicate with a sensor") + +// Device wraps an I2C connection to a MPU6886 device. +type Device struct { + bus drivers.I2C + Address uint16 + aRange uint8 + gRange uint8 +} + +// Config contains settings for filtering, sampling, and modes of operation +type Config struct { + AccelRange uint8 + GyroRange uint8 +} + +// New creates a new MPU6886 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: bus, Address: DefaultAddress} +} + +// Connected returns whether a MPU6886 has been found. +// It does a "who am I" request and checks the response. +func (d *Device) Connected() bool { + data := []byte{0} + d.bus.Tx(d.Address, []byte{WHO_AM_I}, data) + return data[0] == WhoAmI +} + +// Configure sets up the device for communication. +func (d *Device) Configure(config Config) (err error) { + if config.AccelRange < 4 { + d.aRange = config.AccelRange + } + if config.GyroRange < 4 { + d.gRange = config.GyroRange + } + + if !d.Connected() { + return errNotConnected + } + // This initialization sequence is borrowed from Arduino M5Stack library + // Zero register + if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x00}, nil); err != nil { + return + } + time.Sleep(10 * time.Millisecond) + // Set DEVICE_RESET bit + if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x80}, nil); err != nil { + return + } + time.Sleep(10 * time.Millisecond) + // Set CLKSEL to 1 - Auto selects the best available clock source + if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x01}, nil); err != nil { + return + } + time.Sleep(10 * time.Millisecond) + // Set ACCEL_FS_SEL + if err = d.bus.Tx(d.Address, []byte{ACCEL_CONFIG, d.aRange << 3}, nil); err != nil { + return + } + time.Sleep(time.Millisecond) + // Set FS_SEL + if err = d.bus.Tx(d.Address, []byte{GYRO_CONFIG, d.gRange << 3}, nil); err != nil { + return + } + time.Sleep(time.Millisecond) + // default: 0x80, set DLPF_CFG to 001 (Low Pass Filter) + if err = d.bus.Tx(d.Address, []byte{CONFIG, 0x01}, nil); err != nil { + return + } + time.Sleep(time.Millisecond) + // Set sample rate divisor, sample rate is ~ 170 Hz + if err = d.bus.Tx(d.Address, []byte{SMPLRT_DIV, 0x05}, nil); err != nil { + return + } + time.Sleep(time.Millisecond) + // Set Interupt pin + if err = d.bus.Tx(d.Address, []byte{INT_PIN_CFG, 0x22}, nil); err != nil { + return + } + time.Sleep(time.Millisecond) + // Enable DATA_RDY_INT_EN + if err = d.bus.Tx(d.Address, []byte{INT_ENABLE, 0x01}, nil); err != nil { + return + } + time.Sleep(100 * time.Millisecond) + return nil +} + +// ReadTemperature returns the temperature in Celsius millidegrees (°C/1000). +func (d *Device) ReadTemperature() (t int32, err error) { + data := make([]byte, 2) + if err = d.bus.Tx(d.Address, []byte{TEMP_OUT_H}, data); err != nil { + return + } + rawTemperature := int32(int16((uint16(data[0]) << 8) | uint16(data[1]))) + // The formula to convert to degrre of Celsius is + // T_C = T_raw / 326.8 + 25.0 + // This formula should not overflow + t = rawTemperature*10000/3268 + 25000 + return +} + +// 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, err error) { + data := make([]byte, 6) + if err = d.bus.Tx(d.Address, []byte{ACCEL_XOUT_H}, data); err != nil { + return + } + // 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 + divider := int32(1) + switch d.aRange { + case AFS_RANGE_2_G: + divider = 256 + case AFS_RANGE_4_G: + divider = 128 + case AFS_RANGE_8_G: + divider = 64 + case AFS_RANGE_16_G: + divider = 32 + } + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / divider + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / divider + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / divider + 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 +// 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, err error) { + data := make([]byte, 6) + if err = d.bus.Tx(d.Address, []byte{GYRO_XOUT_H}, data); err != nil { + return + } + // 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. + divider := int32(1) + switch d.gRange { + case GFS_RANGE_250: + divider = 2048 + case GFS_RANGE_500: + divider = 1024 + case GFS_RANGE_1000: + divider = 512 + case GFS_RANGE_2000: + divider = 256 + } + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / divider * 1000 + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / divider * 1000 + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / divider * 1000 + return +} diff --git a/mpu6886/registers.go b/mpu6886/registers.go new file mode 100644 index 0000000..1f7b148 --- /dev/null +++ b/mpu6886/registers.go @@ -0,0 +1,116 @@ +package mpu6886 + +// Constants/addresses used for I2C. + +// The I2C address which this device listens to. +const ( + DefaultAddress = 0x68 + SecondaryAddress = 0x69 +) + +// Registers. Names, addresses and comments copied from the datasheet. +const ( + XG_OFFS_TC_H = 0x04 + XG_OFFS_TC_L = 0x05 + YG_OFFS_TC_H = 0x07 + YG_OFFS_TC_L = 0x08 + ZG_OFFS_TC_H = 0x0A + ZG_OFFS_TC_L = 0x0B + + // Self test registers + SELF_TEST_X_ACCEL = 0x0D + SELF_TEST_Y_ACCEL = 0x0E + SELF_TEST_Z_ACCEL = 0x0F + + XG_OFFS_USRH = 0x13 + XG_OFFS_USRL = 0x14 + YG_OFFS_USRH = 0x15 + YG_OFFS_USRL = 0x16 + ZG_OFFS_USRH = 0x17 + ZG_OFFS_USRL = 0x18 + + SMPLRT_DIV = 0x19 + CONFIG = 0x1A + GYRO_CONFIG = 0x1B + ACCEL_CONFIG = 0x1C + ACCEL_CONFIG_2 = 0x1D + LP_MODE_CFG = 0x1E + ACCEL_WOM_X_THR = 0x20 + ACCEL_WOM_Y_THR = 0x21 + ACCEL_WOM_Z_THR = 0x22 + FIFO_EN = 0x23 + FSYNC_INT = 0x36 + + // Interrupt configuration + INT_PIN_CFG = 0x37 + INT_ENABLE = 0x38 + FIFO_WM_INT_STATUS = 0x39 + INT_STATUS = 0x3A + + // 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 + + // Temperature measurement + 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 + + SELF_TEST_X_GYRO = 0x50 + SELF_TEST_Y_GYRO = 0x51 + SELF_TEST_Z_GYRO = 0x52 + + E_ID0 = 0x53 + E_ID1 = 0x54 + E_ID2 = 0x55 + E_ID3 = 0x56 + E_ID4 = 0x57 + E_ID5 = 0x58 + E_ID6 = 0x59 + + FIFO_WM_TH1 = 0x60 + FIFO_WM_TH2 = 0x61 + SIGNAL_PATH_RESET = 0x68 + ACCEL_INTEL_CTRL = 0x69 + USER_CTRL = 0x6A + PWR_MGMT_1 = 0x6B + PWR_MGMT_2 = 0x6C + I2C_IF = 0x70 + FIFO_COUNTH = 0x72 + FIFO_COUNTL = 0x73 + FIFO_R_W = 0x74 + WHO_AM_I = 0x75 + + XA_OFFSET_H = 0x77 + XA_OFFSET_L = 0x78 + YA_OFFSET_H = 0x7A + YA_OFFSET_L = 0x7B + ZA_OFFSET_H = 0x7D + ZA_OFFSET_L = 0x7E +) + +// Accelerometer and gyroscope ranges +const ( + AFS_RANGE_2_G = iota + AFS_RANGE_4_G + AFS_RANGE_8_G + AFS_RANGE_16_G +) + +const ( + GFS_RANGE_250 = iota + GFS_RANGE_500 + GFS_RANGE_1000 + GFS_RANGE_2000 +)