From 8d60b1c706531202123633693e40f387362787ac Mon Sep 17 00:00:00 2001 From: soypat Date: Sun, 29 Aug 2021 11:47:06 -0300 Subject: [PATCH] drivers: add mpu6050 additional functionality --- mpu6050/io.go | 15 +++ mpu6050/mpu6050.go | 2 +- mpu6050/mpu6050_stored.go | 242 ++++++++++++++++++++++++++++++++++++++ mpu6050/registers.go | 47 ++++++++ 4 files changed, 305 insertions(+), 1 deletion(-) create mode 100644 mpu6050/io.go create mode 100644 mpu6050/mpu6050_stored.go diff --git a/mpu6050/io.go b/mpu6050/io.go new file mode 100644 index 000000000..103cb1826 --- /dev/null +++ b/mpu6050/io.go @@ -0,0 +1,15 @@ +package mpu6050 + +// Read reads a single register with minimal heap allocations +func (d *DeviceStored) read(reg uint8) (byte, error) { + d.rreg[0] = reg + err := d.bus.Tx(uint16(d.Address), d.rreg[:1], d.buf[:1]) + return d.buf[0], err +} + +// Write writes a single register. +func (d *DeviceStored) write(reg uint8, data byte) (err error) { + d.buf[0] = reg + d.buf[1] = data + return d.bus.Tx(d.Address, d.buf[:2], nil) +} diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go index f92022237..4f9bed360 100644 --- a/mpu6050/mpu6050.go +++ b/mpu6050/mpu6050.go @@ -20,7 +20,7 @@ type Device struct { // // This function only creates the Device object, it does not touch the device. func New(bus drivers.I2C) Device { - return Device{bus, Address} + return Device{bus: bus, Address: Address} } // Connected returns whether a MPU6050 has been found. diff --git a/mpu6050/mpu6050_stored.go b/mpu6050/mpu6050_stored.go new file mode 100644 index 000000000..b3cfe0a72 --- /dev/null +++ b/mpu6050/mpu6050_stored.go @@ -0,0 +1,242 @@ +package mpu6050 + +import ( + "errors" + "time" + + "tinygo.org/x/drivers" +) + +var ( + ErrInvalidGyroFSR = errors.New("mpu6050:invalid gyro range") + ErrInvalidAccelFSR = errors.New("mpu6050:invalid accel range") + ErrInvalidClkSel = errors.New("mpu6050:invalid clock source") + ErrInvalidDLPF = errors.New("mpu6050:invalid DLPF") +) + +// DeviceStored reads all IMU data in a single i2c transaction +// to reduce bus usage. It is a better alternative to a simple Device +// if getting gyroscopes+acceleration measurements together many times in +// a second. +type DeviceStored struct { + Device + gyroRange int16 + accelRange int16 + // buf is buffer for small reads/writes + buf [2]byte + // rreg stores read register + rreg [1]byte + // data contains IMU readings + data [14]byte +} + +// NewStored creates a versatile instance of a mpu6050 device handle. +// Use Frequencies above 40kHz to avoid timeout issues. +func NewStored(bus drivers.I2C) DeviceStored { + return DeviceStored{Device: New(bus), accelRange: 2, gyroRange: 250} +} + +type Config struct { + // Set to one of ACCEL_RANGE_X register values + AccRange byte + // Set to one of GYRO_RANGE_x register values + GyroRange byte + // SetSampleRate sets the sample rate for the FIFO, + // register ouput and DMP. The sample rate is determined + // by: + // SR = Gyroscope Output Rate / (1 + srDiv) + SampleRatio byte + ClkSel byte +} + +// 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 (d *DeviceStored) Configure(data Config) (err error) { + // d.Reset() + + d.Device.Configure() + // setClockSource + if err = d.SetClockSource(data.ClkSel); err != nil { + return err + } + // setSampleRate + if err = d.SetSampleRate(data.SampleRatio); err != nil { + return err + } + // setFullScaleGyroRange + if err = d.SetGyroRange(data.GyroRange); err != nil { + return err + } + // setFullScaleAccelRange + if err = d.SetAccelRange(data.AccRange); err != nil { + return err + } + // setSleep + return d.SetSleep(false) // wake MPU6050 up +} + +// 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 *DeviceStored) Acceleration() (ax, ay, az int32) { + const accelOffset = 0 + return int32(int16((uint16(d.data[accelOffset])<<8)|uint16(d.data[accelOffset+1]))) * 15625 / 512 * int32(d.accelRange), + int32(int16((uint16(d.data[accelOffset+2])<<8)|uint16(d.data[accelOffset+3]))) * 15625 / 512 * int32(d.accelRange), + int32(int16((uint16(d.data[accelOffset+4])<<8)|uint16(d.data[accelOffset+5]))) * 15625 / 512 * int32(d.accelRange) +} + +// Rotations 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 *DeviceStored) Rotation() (gx, gy, gz int32) { + const angvelOffset = 8 + return int32(int16((uint16(d.data[angvelOffset])<<8)|uint16(d.data[angvelOffset+1]))) * 15625 / 2048 * int32(d.gyroRange) * 4, + int32(int16((uint16(d.data[angvelOffset+2])<<8)|uint16(d.data[angvelOffset+3]))) * 15625 / 2048 * int32(d.gyroRange) * 4, + int32(int16((uint16(d.data[angvelOffset+4])<<8)|uint16(d.data[angvelOffset+5]))) * 15625 / 2048 * int32(d.gyroRange) * 4 +} + +// Temperature returns the temperature of the device in centigrade. +func (d *DeviceStored) Temperature() (Celsius int16) { + const tempOffset = 6 + return ((int16(d.data[tempOffset])<<8)|int16(d.data[tempOffset+1]))/340 + 37 // float64(temp/340) + 36.53 +} + +// Get reads IMU data and stores it inside DeviceStored. The data can then be accessed through Rotation and Acceleration +// methods. +func (d *DeviceStored) Get() error { + d.rreg[0] = ACCEL_XOUT_H + return d.bus.Tx(d.Address, d.rreg[:1], d.data[:14]) +} + +// 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 (d *DeviceStored) SetSampleRate(srDiv byte) (err error) { + return d.write(SMPLRT_DIV, srDiv) +} + +// SetClockSource configures the source of the clock +// for the peripheral. When the MPU6050 starts up it uses it's own +// clock until configured otherwise. +// +// 0: Internal 8MHz Oscillator +// 1: PLL with X axis gyroscope reference +// 2: PLL with Y axis gyroscope reference +// 3: PLL with Z axis gyroscope reference +// 4: PLL with external 32.768kHz reference +// 5: PLL with external 19.2MHz reference +// 6: reserved +// 7: Stops the clock and keeps the timing generator in reset +func (d *DeviceStored) SetClockSource(clkSel byte) error { + if clkSel == 6 || clkSel > 7 { + return ErrInvalidClkSel + } + regdata, err := d.read(PWR_MGMT_1) + if err != nil { + return err + } + regdata = (regdata &^ CLK_SEL_Msk) | clkSel // Write CLKSEL field + return d.write(PWR_MGMT_1, regdata) +} + +// SetGyroRange configures the full scale range of the gyroscope. +// It has four possible values +- 250°/s, 500°/s, 1000°/s, 2000°/s. +// The function takes values of gyroRange from 0-3 where 0 means the +// lowest FSR (250°/s) and 3 is the highest FSR (2000°/s). +func (d *DeviceStored) SetGyroRange(gyroRange byte) (err error) { + switch gyroRange { + case GYRO_RANGE_250: + d.gyroRange = 250 + case GYRO_RANGE_500: + d.gyroRange = 500 + case GYRO_RANGE_1000: + d.gyroRange = 1000 + case GYRO_RANGE_2000: + d.gyroRange = 2000 + default: + return ErrInvalidGyroFSR + } + // setFullScaleGyroRange + regdata, err := d.read(GYRO_CONFIG) + if err != nil { + return err + } + + regdata = (regdata &^ G_FS_SEL) | (gyroRange << GFS_Pos) // Write FS_SEL field + return d.write(GYRO_CONFIG, regdata) +} + +// SetAccelRange 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 (d *DeviceStored) SetAccelRange(accRange byte) (err error) { + switch accRange { + case ACCEL_RANGE_2: + d.accelRange = 2 + case ACCEL_RANGE_4: + d.accelRange = 4 + case ACCEL_RANGE_8: + d.accelRange = 8 + case ACCEL_RANGE_16: + d.accelRange = 16 + default: + return ErrInvalidAccelFSR + } + regdata, err := d.read(ACCEL_CONFIG) + if err != nil { + return err + } + regdata = (regdata &^ AFS_SEL) | (accRange << AFS_Pos) // Write only FS_SEL field + return d.write(ACCEL_CONFIG, regdata) +} + +// Set filter bandwidth. Has side effect of reducing the sample rate +// with higher low pass filter values. +func (d *DeviceStored) SetDigitalLowPass(dlpf byte) error { + if dlpf >= 7 { + return ErrInvalidDLPF + } + regdata, err := d.read(CONFIG) + if err != nil { + return err + } + return d.write(CONFIG, (regdata&^DLPF_Msk)|dlpf) +} + +// SetSleep 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 (d *DeviceStored) SetSleep(sleep bool) (err error) { + regdata, err := d.read(PWR_MGMT_1) + if err != nil { + return err + } + regdata &^= SLEEP_Msk + if sleep { + regdata |= (1 << SLEEP_Pos) // Set CLK_SEL bits only + } + return d.write(PWR_MGMT_1, regdata) +} + +func (d *DeviceStored) Reset() error { + err := d.write(PWR_MGMT_1, RESET_Byte) + time.Sleep(100 * time.Millisecond) + return err +} + +func DefaultConfig() Config { + return Config{AccRange: ACCEL_RANGE_2, GyroRange: GYRO_RANGE_250} +} diff --git a/mpu6050/registers.go b/mpu6050/registers.go index 1206002e9..9cb4f3317 100644 --- a/mpu6050/registers.go +++ b/mpu6050/registers.go @@ -19,6 +19,7 @@ const ( ACCEL_CONFIG = 0x1C // Accelerometer configuration FIFO_EN = 0x23 // FIFO enable + RESET_Byte = 1 << 7 // I2C pass-through configuration I2C_MST_CTRL = 0x24 I2C_SLV0_ADDR = 0x25 @@ -107,3 +108,49 @@ const ( FIFO_R_W = 0x74 // FIFO read/write WHO_AM_I = 0x75 // Who am I ) + +//MPU 6050 MASKS +const ( + G_FS_SEL uint8 = 0x18 + AFS_SEL uint8 = 0x18 + CLK_SEL_Msk uint8 = 0x07 + SLEEP_Msk uint8 = 0x40 +) + +//MPU 6050 SHIFTS +const ( + AFS_Pos = 3 + GFS_Pos = 3 + SLEEP_Pos = 6 +) + +// Gyroscope ranges for Init configuration. Full Scale Ranges. +const ( + GYRO_RANGE_250 byte = iota + GYRO_RANGE_500 + GYRO_RANGE_1000 + GYRO_RANGE_2000 +) + +// Accelerometer ranges for Init configuration +const ( + ACCEL_RANGE_2 byte = iota + ACCEL_RANGE_4 + ACCEL_RANGE_8 + ACCEL_RANGE_16 +) + +// DLPF Digital Low Pass Filter register values. CONFIG register. +const ( + DLPF_None = iota + // 2ms measurement delay. 188Hz bandwidth + DLPF_BW188 + DLPF_BW98 + DLPF_BW44 + DLPF_BW21 + DLPF_BW10 + // 18ms delay. 10Hz bandwidth + DLPF_BW5 + DLPF_Msk = 0b111 + DLPF_Pos = 0 +)