Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Mpu sensor rewrite 2 #577

Open
wants to merge 1 commit into
base: dev
Choose a base branch
from
Open

Mpu sensor rewrite 2 #577

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 18 additions & 4 deletions examples/mpu6050/main.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,18 +5,32 @@ import (
"machine"
"time"

"tinygo.org/x/drivers"
"tinygo.org/x/drivers/mpu6050"
)

func main() {
machine.I2C0.Configure(machine.I2CConfig{})

accel := mpu6050.New(machine.I2C0)
accel.Configure()
mpuDevice := mpu6050.New(machine.I2C0, mpu6050.DefaultAddress)

// Configure the device with default configuration.
err := mpuDevice.Configure(mpu6050.Config{})
if err != nil {
panic(err.Error())
}
for {
x, y, z := accel.ReadAcceleration()
println(x, y, z)
time.Sleep(time.Millisecond * 100)
err := mpuDevice.Update(drivers.AllMeasurements)
if err != nil {
println("error reading from mpu6050:", err.Error())
continue
}
print("acceleration: ")
println(mpuDevice.Acceleration())
print("angular velocity:")
println(mpuDevice.AngularVelocity())
print("temperature celsius:")
println(mpuDevice.Temperature() / 1000)
}
}
284 changes: 210 additions & 74 deletions mpu6050/mpu6050.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,90 +6,226 @@
// https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
package mpu6050 // import "tinygo.org/x/drivers/mpu6050"

import "tinygo.org/x/drivers"
import (
"encoding/binary"
"errors"

// Device wraps an I2C connection to a MPU6050 device.
"tinygo.org/x/drivers"
)

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.
AccelRange RangeAccel
// Use GYRO_RANGE_250 through GYRO_RANGE_2000
GyroRange RangeGyro
sampleRatio byte // TODO(soypat): expose these as configurable.
clkSel byte
}

// Compile-time guarantee that Device implements the drivers.Sensor interface.
var _ drivers.Sensor = &Device{}

// Device contains MPU board abstraction for usage
type Device struct {
bus drivers.I2C
Address uint16
conn drivers.I2C
aRange int32 //Gyroscope FSR acording to SetAccelRange input
gRange int32 //Gyroscope FSR acording to SetGyroRange input
// 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
}

// 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 drivers.I2C) Device {
return Device{bus, Address}
// New instantiates and initializes a MPU6050 struct without writing/reading
// i2c bus. Typical I2C MPU6050 address is 0x68.
func New(bus drivers.I2C, addr uint16) *Device {
p := &Device{}
p.address = uint8(addr)
p.conn = bus
return p
}

// 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 (p *Device) Configure(data Config) (err error) {
if err = p.Sleep(false); err != nil {
return err
}
if err = p.setClockSource(data.clkSel); err != nil {
return err
}
if err = p.setSampleRate(data.sampleRatio); err != nil {
return err
}
if err = p.setRangeGyro(data.GyroRange); err != nil {
return err
}
if err = p.setRangeAccel(data.AccelRange); err != nil {
return err
}
return nil
}

// 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)
d.read(_WHO_AM_I, data)
return data[0] == 0x68
}

// Configure sets up the device for communication.
func (d Device) Configure() error {
return d.SetClockSource(CLOCK_INTERNAL)
}

// 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
// Update fetches the latest data from the MPU6050
func (p *Device) Update(which drivers.Measurement) (err error) {
// TODO(soypat): This could be optimized to read only the requested data.
// Should have benchmarks to show the difference.
const available = drivers.Acceleration | drivers.AngularVelocity | drivers.Temperature
if which&available == 0 {
return nil // No relevant data requested.
}
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.
func (d *Device) Acceleration() (ax, ay, az int32) {
const accelOffset = 0
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
}

// 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 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
}

// SetClockSource allows the user to configure the clock source.
func (d Device) SetClockSource(source uint8) error {
return d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{source})
}

// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope.
func (d Device) SetFullScaleGyroRange(rng uint8) error {
return d.bus.WriteRegister(uint8(d.Address), GYRO_CONFIG, []uint8{rng})
}

// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer.
func (d Device) SetFullScaleAccelRange(rng uint8) error {
return d.bus.WriteRegister(uint8(d.Address), ACCEL_CONFIG, []uint8{rng})
// you would get a value close to 6.3 radians (360 degrees).
func (d *Device) AngularVelocity() (gx, gy, gz int32) {
const angvelOffset = 8
_ = 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.data[tempOffset:]))/512 + 37*1000
}

func convertWord(buf []byte) int16 {
return int16(binary.BigEndian.Uint16(buf))
}

// 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 (p *Device) setSampleRate(srDiv byte) (err error) {
// setSampleRate
var sr [1]byte
sr[0] = srDiv
if err = p.write8(_SMPRT_DIV, sr[0]); err != nil {
return err
}
return nil
}

// setClockSource configures the source of the clock
// for the peripheral.
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.
// It has four possible values +- 250°/s, 500°/s, 1000°/s, 2000°/s.
func (p *Device) setRangeGyro(gyroRange RangeGyro) (err error) {
switch gyroRange {
case RangeGyro250:
p.gRange = 250
case RangeGyro500:
p.gRange = 500
case RangeGyro1000:
p.gRange = 1000
case RangeGyro2000, rangeGyroDefault:
gyroRange = RangeGyro2000
p.gRange = 2000
default:
return errInvalidRangeGyro
}
return p.writeMasked(_GYRO_CONFIG, _G_FS_SEL, uint8(gyroRange-1)<<_G_FS_SHIFT)
}

// 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 RangeAccel) (err error) {
switch accRange {
case RangeAccel2:
p.aRange = 2
case RangeAccel4:
p.aRange = 4
case RangeAccel8:
p.aRange = 8
case RangeAccel16, rangeAccelDefault:
accRange = RangeAccel16
p.aRange = 16
default:
return errInvalidRangeAccel
}
return p.writeMasked(_ACCEL_CONFIG, _AFS_SEL, uint8(accRange-1)<<_AFS_SHIFT)
}

// Sleep 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 (p *Device) Sleep(sleepEnabled bool) (err error) {
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
}
b[0] = (b[0] &^ mask) | value&mask
return d.write8(reg, b[0])
}

func b2u8(b bool) byte {
if b {
return 1
}
return 0
}
Loading