|
| 1 | +// Package mpu9150 provides a driver for the MPU9150 accelerometer and gyroscope |
| 2 | +// made by InvenSense. |
| 3 | +// |
| 4 | +// Datasheets: |
| 5 | +// https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf |
| 6 | +// https://inertialelements.com/documents/resources_page/MPU9150-register-manual.pdf |
| 7 | + |
| 8 | +package mpu9150 |
| 9 | + |
| 10 | +import ( |
| 11 | + "tinygo.org/x/drivers" |
| 12 | + "tinygo.org/x/drivers/internal/legacy" |
| 13 | +) |
| 14 | + |
| 15 | +// Device wraps an I2C connection to a MPU9150 device. |
| 16 | +type Device struct { |
| 17 | + bus drivers.I2C |
| 18 | + Address uint16 |
| 19 | +} |
| 20 | + |
| 21 | +// New creates a new MPU9150 connection. The I2C bus must already be |
| 22 | +// configured. |
| 23 | +// |
| 24 | +// This function only creates the Device object, it does not touch the device. |
| 25 | +func New(bus drivers.I2C) Device { |
| 26 | + return Device{bus, Address} |
| 27 | +} |
| 28 | + |
| 29 | +// Connected returns whether a MPU9150 has been found. |
| 30 | +// It does a "who am I" request and checks the response. |
| 31 | +func (d Device) Connected() bool { |
| 32 | + data := []byte{0} |
| 33 | + legacy.ReadRegister(d.bus, uint8(d.Address), WHO_AM_I, data) |
| 34 | + return data[0] == 0x68 // 4.32 Register 117 – Who Am I (MPU-9150 Register Map and Descriptions) |
| 35 | +} |
| 36 | + |
| 37 | +// Configure sets up the device for communication. |
| 38 | +func (d Device) Configure() error { |
| 39 | + return d.SetClockSource(CLOCK_INTERNAL) |
| 40 | +} |
| 41 | + |
| 42 | +// ReadAcceleration reads the current acceleration from the device and returns |
| 43 | +// it in µg (micro-gravity). When one of the axes is pointing straight to Earth |
| 44 | +// and the sensor is not moving the returned value will be around 1000000 or |
| 45 | +// -1000000. |
| 46 | +func (d Device) ReadAcceleration(accel_axis byte) (x int32, y int32, z int32) { |
| 47 | + data := make([]byte, 6) |
| 48 | + legacy.ReadRegister(d.bus, uint8(d.Address), accel_axis, data) |
| 49 | + // Now do two things: |
| 50 | + // 1. merge the two values to a 16-bit number (and cast to a 32-bit integer) |
| 51 | + // 2. scale the value to bring it in the -1000000..1000000 range. |
| 52 | + // This is done with a trick. What we do here is essentially multiply by |
| 53 | + // 1000000 and divide by 16384 to get the original scale, but to avoid |
| 54 | + // overflow we do it at 1/64 of the value: |
| 55 | + // 1000000 / 64 = 15625 |
| 56 | + // 16384 / 64 = 256 |
| 57 | + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256 |
| 58 | + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256 |
| 59 | + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256 |
| 60 | + return |
| 61 | +} |
| 62 | + |
| 63 | +// ReadRotation reads the current rotation from the device and returns it in |
| 64 | +// µ°/s (micro-degrees/sec). This means that if you were to do a complete |
| 65 | +// rotation along one axis and while doing so integrate all values over time, |
| 66 | +// you would get a value close to 360000000. |
| 67 | +func (d Device) ReadRotation(gyro_axis byte) (x int32, y int32, z int32) { |
| 68 | + data := make([]byte, 6) |
| 69 | + legacy.ReadRegister(d.bus, uint8(d.Address), gyro_axis, data) |
| 70 | + // First the value is converted from a pair of bytes to a signed 16-bit |
| 71 | + // value and then to a signed 32-bit value to avoid integer overflow. |
| 72 | + // Then the value is scaled to µ°/s (micro-degrees per second). |
| 73 | + // This is done in the following steps: |
| 74 | + // 1. Multiply by 250 * 1000_000 |
| 75 | + // 2. Divide by 32768 |
| 76 | + // The following calculation (x * 15625 / 2048 * 1000) is essentially the |
| 77 | + // same but avoids overflow. First both operations are divided by 16 leading |
| 78 | + // to multiply by 15625000 and divide by 2048, and then part of the multiply |
| 79 | + // is done after the divide instead of before. |
| 80 | + x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000 |
| 81 | + y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000 |
| 82 | + z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000 |
| 83 | + return |
| 84 | +} |
| 85 | + |
| 86 | +// SetClockSource allows the user to configure the clock source. |
| 87 | +func (d Device) SetClockSource(source uint8) error { |
| 88 | + return legacy.WriteRegister(d.bus, uint8(d.Address), PWR_MGMT_1, []uint8{source}) |
| 89 | +} |
| 90 | + |
| 91 | +// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope. |
| 92 | +func (d Device) SetFullScaleGyroRange(rng uint8) error { |
| 93 | + return legacy.WriteRegister(d.bus, uint8(d.Address), GYRO_CONFIG, []uint8{rng}) |
| 94 | +} |
| 95 | + |
| 96 | +// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer. |
| 97 | +func (d Device) SetFullScaleAccelRange(rng uint8) error { |
| 98 | + return legacy.WriteRegister(d.bus, uint8(d.Address), ACCEL_CONFIG, []uint8{rng}) |
| 99 | +} |
0 commit comments