69 lines
1.1 KiB
Go
69 lines
1.1 KiB
Go
|
package mpu6050
|
||
|
|
|
||
|
|
import (
|
||
|
|
"fmt"
|
||
|
|
|
||
|
|
"golang.org/x/exp/io/i2c"
|
||
|
|
)
|
||
|
|
|
||
|
|
const (
|
||
|
|
mpuAddr = 0x68
|
||
|
|
|
||
|
|
// MPU registers
|
||
|
|
|
||
|
|
// Power management
|
||
|
|
// powerMgmt1 = 0x6B
|
||
|
|
// powerMgmt2 = 0x6C
|
||
|
|
// Acceleration
|
||
|
|
// accelXOut0 = 0x3B
|
||
|
|
// accelYOut0 = 0x3D
|
||
|
|
// accelZOut0 = 0x3F
|
||
|
|
// Temperature
|
||
|
|
tempOut0 = 0x41
|
||
|
|
|
||
|
|
// Gyro
|
||
|
|
// gyroXOut0 = 0x43
|
||
|
|
// gyroYOut0 = 0x45
|
||
|
|
// gyroZOut0 = 0x47
|
||
|
|
// Configs
|
||
|
|
// accelConfig = 0x1C
|
||
|
|
// gyroConfig = 0x1B
|
||
|
|
// mpuConfig = 0x1A
|
||
|
|
)
|
||
|
|
|
||
|
|
type device struct {
|
||
|
|
device *i2c.Device
|
||
|
|
}
|
||
|
|
|
||
|
|
type Device interface {
|
||
|
|
GetTemp() (float32, error)
|
||
|
|
GetAccel() ([3]float32, error)
|
||
|
|
GetGyro() ([3]float32, error)
|
||
|
|
}
|
||
|
|
|
||
|
|
func Open() (Device, error) {
|
||
|
|
d, err := i2c.Open(&i2c.Devfs{Dev: "/dev/i2c-1"}, mpuAddr)
|
||
|
|
if err != nil {
|
||
|
|
return nil, fmt.Errorf("open i2c: %w", err)
|
||
|
|
}
|
||
|
|
return &device{
|
||
|
|
device: d,
|
||
|
|
}, nil
|
||
|
|
}
|
||
|
|
|
||
|
|
func (d *device) GetTemp() (float32, error) {
|
||
|
|
rawTemp, err := d.readWord(tempOut0)
|
||
|
|
if err != nil {
|
||
|
|
return 0, err
|
||
|
|
}
|
||
|
|
return (float32(rawTemp) / 340.0) + 36.53, nil
|
||
|
|
}
|
||
|
|
|
||
|
|
func (d *device) GetAccel() ([3]float32, error) {
|
||
|
|
return [3]float32{}, nil
|
||
|
|
}
|
||
|
|
|
||
|
|
func (d *device) GetGyro() ([3]float32, error) {
|
||
|
|
return [3]float32{}, nil
|
||
|
|
}
|