mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-08-06 07:53:41 +00:00
Ayke van Laethem
78fdaad9c0
Units were unspecified before but were in practice the raw output from the sensor. They have now been changed to sensible outputs understandable to humans, like the other accelerometers.
84 lines
3.3 KiB
Go
84 lines
3.3 KiB
Go
// 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 (
|
|
"machine"
|
|
)
|
|
|
|
// Device wraps an I2C connection to a MPU6050 device.
|
|
type Device struct {
|
|
bus machine.I2C
|
|
Address uint16
|
|
}
|
|
|
|
// 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 machine.I2C) Device {
|
|
return Device{bus, Address}
|
|
}
|
|
|
|
// 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)
|
|
return data[0] == 0x68
|
|
}
|
|
|
|
// Configure sets up the device for communication.
|
|
func (d Device) Configure() {
|
|
d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{0})
|
|
}
|
|
|
|
// 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
|
|
// 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
|
|
}
|