diff --git a/examples/mpu6050/main.go b/examples/mpu6050/main.go index 7e76ef40c..b64fe3be6 100644 --- a/examples/mpu6050/main.go +++ b/examples/mpu6050/main.go @@ -14,8 +14,8 @@ func main() { mpuDevice := mpu6050.New(machine.I2C0, mpu6050.DefaultAddress) err := mpuDevice.Configure(mpu6050.Config{ - AccRange: mpu6050.ACCEL_RANGE_16, - GyroRange: mpu6050.GYRO_RANGE_2000, + AccelRange: mpu6050.ACCEL_RANGE_16, + GyroRange: mpu6050.GYRO_RANGE_2000, }) if err != nil { panic(err.Error()) @@ -31,7 +31,7 @@ func main() { println(mpuDevice.Acceleration()) print("angular velocity:") println(mpuDevice.AngularVelocity()) - print("temperature centigrade:") + print("temperature celsius:") println(mpuDevice.Temperature() / 1000) } } diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go index cb8f7deca..b7f9d3698 100644 --- a/mpu6050/mpu6050.go +++ b/mpu6050/mpu6050.go @@ -1,4 +1,10 @@ -package mpu6050 +// Package mpu6050 provides a driver for the MPU6050 accelerometer and gyroscope +// made by InvenSense. +// +// Datasheets: +// https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf +// https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf +package mpu6050 // import "tinygo.org/x/drivers/mpu6050" import ( "encoding/binary" @@ -9,11 +15,24 @@ import ( 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. - AccRange byte + AccelRange RangeAccel // Use GYRO_RANGE_250 through GYRO_RANGE_2000 - GyroRange byte + GyroRange RangeGyro sampleRatio byte // TODO(soypat): expose these as configurable. clkSel byte } @@ -23,9 +42,12 @@ type Device struct { conn drivers.I2C aRange int32 //Gyroscope FSR acording to SetAccelRange input gRange int32 //Gyroscope FSR acording to SetGyroRange input - // RawData contains the accelerometer, gyroscope and temperature RawData read - // in the last call via the Update method. - RawData [14]byte + // 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 } @@ -44,67 +66,72 @@ func New(bus drivers.I2C, addr uint16) *Device { // and wakes up the peripheral. func (p *Device) Configure(data Config) (err error) { if err = p.Sleep(false); err != nil { - return errors.New("set sleep: " + err.Error()) + return err } - if err = p.SetClockSource(data.clkSel); err != nil { - return errors.New("set clksrc: " + err.Error()) + if err = p.setClockSource(data.clkSel); err != nil { + return err } - if err = p.SetSampleRate(data.sampleRatio); err != nil { - return errors.New("set sampleratio: " + err.Error()) + if err = p.setSampleRate(data.sampleRatio); err != nil { + return err } - if err = p.SetRangeGyro(data.GyroRange); err != nil { - return errors.New("set gyrorange: " + err.Error()) + if err = p.setRangeGyro(data.GyroRange); err != nil { + return err } - if err = p.SetRangeAccel(data.AccRange); err != nil { - return errors.New("set accelrange: " + err.Error()) + if err = p.setRangeAccel(data.AccelRange); err != nil { + return err } return nil } +func (d Device) Connected() bool { + data := []byte{0} + d.read(_WHO_AM_I, data) + return data[0] == 0x68 +} + // Update fetches the latest data from the MPU6050 func (p *Device) Update() (err error) { - if err = p.read(_ACCEL_XOUT_H, p.RawData[:]); err != nil { + 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. +// 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.RawData[accelOffset+0:])) * 15625 / 512 * d.aRange - ay = int32(convertWord(d.RawData[accelOffset+2:])) * 15625 / 512 * d.aRange - az = int32(convertWord(d.RawData[accelOffset+4:])) * 15625 / 512 * d.aRange + 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 } -// Rotations reads the current rotation from the device and returns it in +// 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 6.3 radians (360 degrees). func (d *Device) AngularVelocity() (gx, gy, gz int32) { const angvelOffset = 8 - _ = d.RawData[angvelOffset+5] // This line fails to compile if RawData is too short. - gx = int32(convertWord(d.RawData[angvelOffset+0:])) * 4363 / 8192 * d.gRange - gy = int32(convertWord(d.RawData[angvelOffset+2:])) * 4363 / 8192 * d.gRange - gz = int32(convertWord(d.RawData[angvelOffset+4:])) * 4363 / 8192 * d.gRange + _ = 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.RawData[tempOffset:]))/512 + 37*1000 + 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, +// setSampleRate sets the sample rate for the FIFO, // register ouput and DMP. The sample rate is determined // by: // @@ -114,7 +141,7 @@ func convertWord(buf []byte) int16 { // 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) { +func (p *Device) setSampleRate(srDiv byte) (err error) { // setSampleRate var sr [1]byte sr[0] = srDiv @@ -124,81 +151,48 @@ func (p *Device) SetSampleRate(srDiv byte) (err error) { return nil } -// SetClockSource configures the source of the clock +// setClockSource configures the source of the clock // for the peripheral. -func (p *Device) SetClockSource(clkSel byte) (err error) { - // setClockSource - var pwrMgt [1]byte - - if err = p.read(_PWR_MGMT_1, pwrMgt[:]); err != nil { - return err - } - pwrMgt[0] = (pwrMgt[0] & (^_CLK_SEL_MASK)) | clkSel // Escribo solo el campo de clk_sel - if err = p.write8(_PWR_MGMT_1, pwrMgt[0]); err != nil { - return err - } - return nil +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. +// setRangeGyro 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 (p *Device) SetRangeGyro(gyroRange byte) (err error) { +func (p *Device) setRangeGyro(gyroRange RangeGyro) (err error) { switch gyroRange { - case GYRO_RANGE_250: + case RangeGyro250: p.gRange = 250 - case GYRO_RANGE_500: + case RangeGyro500: p.gRange = 500 - case GYRO_RANGE_1000: + case RangeGyro1000: p.gRange = 1000 - case GYRO_RANGE_2000: + case RangeGyro2000: p.gRange = 2000 default: - return errors.New("invalid gyroscope FSR input") + return errInvalidRangeGyro } - // setFullScaleGyroRange - var gConfig [1]byte - - if err = p.read(_GYRO_CONFIG, gConfig[:]); err != nil { - return err - } - gConfig[0] = (gConfig[0] & (^_G_FS_SEL)) | (gyroRange << _G_FS_SHIFT) // Escribo solo el campo de FS_sel - - if err = p.write8(_GYRO_CONFIG, gConfig[0]); err != nil { - return err - } - return nil + return p.writeMasked(_GYRO_CONFIG, _G_FS_SEL, uint8(gyroRange)<<_G_FS_SHIFT) } -// SetRangeAccel configures the full scale range of the accelerometer. +// 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 byte) (err error) { +func (p *Device) setRangeAccel(accRange RangeAccel) (err error) { switch accRange { - case ACCEL_RANGE_2: + case RangeAccel2: p.aRange = 2 - case ACCEL_RANGE_4: + case RangeAccel4: p.aRange = 4 - case ACCEL_RANGE_8: + case RangeAccel8: p.aRange = 8 - case ACCEL_RANGE_16: + case RangeAccel16: p.aRange = 16 default: - return errors.New("invalid accelerometer FSR input") + return errInvalidRangeAccel } - - var aConfig [1]byte - if err = p.read(_ACCEL_CONFIG, aConfig[:]); err != nil { - return err - } - aConfig[0] = (aConfig[0] & (^_AFS_SEL)) | (accRange << _AFS_SHIFT) - - if err = p.write8(_ACCEL_CONFIG, aConfig[0]); err != nil { - return err - } - return nil + return p.writeMasked(_ACCEL_CONFIG, _AFS_SEL, uint8(accRange)<<_AFS_SHIFT) } // Sleep sets the sleep bit on the power managment 1 field. @@ -206,26 +200,29 @@ func (p *Device) SetRangeAccel(accRange byte) (err error) { // 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) { - // setSleepBit - var pwrMgt [1]byte - if err = p.read(_PWR_MGMT_1, pwrMgt[:]); err != nil { + 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 } - if sleepEnabled { - pwrMgt[0] = (pwrMgt[0] & (^_SLEEP_MASK)) | (1 << _SLEEP_SHIFT) // Overwrite only Sleep - } else { - pwrMgt[0] = (pwrMgt[0] & (^_SLEEP_MASK)) - } - if err = p.write8(_PWR_MGMT_1, pwrMgt[0]); 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 nil + return 0 } func DefaultConfig() Config { return Config{ - AccRange: ACCEL_RANGE_16, - GyroRange: GYRO_RANGE_2000, + AccelRange: RangeAccel16, + GyroRange: RangeGyro2000, sampleRatio: 0, // TODO add const values. clkSel: 0, } diff --git a/mpu6050/registers.go b/mpu6050/registers.go index 0b923beeb..e06bf36b6 100644 --- a/mpu6050/registers.go +++ b/mpu6050/registers.go @@ -3,20 +3,16 @@ package mpu6050 // 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 { - return p.conn.Tx(uint16(p.address), []byte{reg}, buff) + buf := [1]byte{reg} + return p.conn.Tx(uint16(p.address), buf[:1], buff) } // write8 writes a registry byte func (p *Device) write8(reg uint8, datum byte) error { - err := p.write(reg, []byte{datum}) - return err -} - -// write write a byte buffer to sequential incrementing adresses (1 byte per address) in individual -// write operations. -func (p *Device) write(addr uint8, buff []byte) error { - p.conn.Tx(uint16(p.address), append([]byte{addr}, buff...), nil) - return nil + var buff [2]byte + buff[0] = reg + buff[1] = datum + return p.conn.Tx(uint16(p.address), buff[:], nil) } // MPU 6050 REGISTER ADRESSES @@ -26,11 +22,6 @@ const ( _GYRO_CONFIG uint8 = 0x1B _ACCEL_CONFIG uint8 = 0x1C _FIFO_EN uint8 = 0x23 - _I2C_MST_CTRL uint8 = 0x24 - _PWR_MGMT_1 uint8 = 0x6B - _ACCEL_XOUT_H uint8 = 0x3B - _TEMP_OUT_H uint8 = 0x41 - _GYRO_XOUT_H uint8 = 0x43 ) // MPU 6050 MASKS @@ -50,16 +41,146 @@ const ( // Gyroscope ranges for Init configuration const ( - GYRO_RANGE_250 byte = 0x00 - GYRO_RANGE_500 byte = 0x01 - GYRO_RANGE_1000 byte = 0x02 - GYRO_RANGE_2000 byte = 0x03 + // 250°/s + RangeGyro250 RangeGyro = iota + // 500°/s + RangeGyro500 + // 1000°/s + RangeGyro1000 + // 2000°/s + RangeGyro2000 ) // Accelerometer ranges for Init configuration const ( - ACCEL_RANGE_2 byte = 0x00 - ACCEL_RANGE_4 byte = 0x01 - ACCEL_RANGE_8 byte = 0x02 - ACCEL_RANGE_16 byte = 0x03 + // 2g + RangeAccel2 RangeAccel = iota + // 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 + + _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 + + // Interrupt configuration + _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 + + // 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 + + // 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 + + // 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 + + // 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 + + // Accelerometer settings + _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 + + // 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 register )