165 lines
4.0 KiB
Go
165 lines
4.0 KiB
Go
package i2c
|
|
|
|
import (
|
|
"bytes"
|
|
"encoding/binary"
|
|
"time"
|
|
|
|
"gobot.io/x/gobot"
|
|
)
|
|
|
|
const mpu6050Address = 0x68
|
|
|
|
const MPU6050_RA_ACCEL_XOUT_H = 0x3B
|
|
const MPU6050_RA_PWR_MGMT_1 = 0x6B
|
|
const MPU6050_PWR1_CLKSEL_BIT = 2
|
|
const MPU6050_PWR1_CLKSEL_LENGTH = 3
|
|
const MPU6050_CLOCK_PLL_XGYRO = 0x01
|
|
const MPU6050_GYRO_FS_250 = 0x00
|
|
const MPU6050_RA_GYRO_CONFIG = 0x1B
|
|
const MPU6050_GCONFIG_FS_SEL_LENGTH = 2
|
|
const MPU6050_GCONFIG_FS_SEL_BIT = 4
|
|
const MPU6050_RA_ACCEL_CONFIG = 0x1C
|
|
const MPU6050_ACONFIG_AFS_SEL_BIT = 4
|
|
const MPU6050_ACONFIG_AFS_SEL_LENGTH = 2
|
|
const MPU6050_ACCEL_FS_2 = 0x00
|
|
const MPU6050_PWR1_SLEEP_BIT = 6
|
|
const MPU6050_PWR1_ENABLE_BIT = 0
|
|
|
|
type ThreeDData struct {
|
|
X int16
|
|
Y int16
|
|
Z int16
|
|
}
|
|
|
|
// MPU6050Driver is a new Gobot Driver for an MPU6050 I2C Accelerometer/Gyroscope.
|
|
type MPU6050Driver struct {
|
|
name string
|
|
connector Connector
|
|
connection Connection
|
|
Config
|
|
interval time.Duration
|
|
Accelerometer ThreeDData
|
|
Gyroscope ThreeDData
|
|
Temperature int16
|
|
gobot.Eventer
|
|
}
|
|
|
|
// NewMPU6050Driver creates a new Gobot Driver for an MPU6050 I2C Accelerometer/Gyroscope.
|
|
//
|
|
// Params:
|
|
// conn Connector - the Adaptor to use with this Driver
|
|
//
|
|
// Optional params:
|
|
// i2c.WithBus(int): bus to use with this driver
|
|
// i2c.WithAddress(int): address to use with this driver
|
|
//
|
|
func NewMPU6050Driver(a Connector, options ...func(Config)) *MPU6050Driver {
|
|
m := &MPU6050Driver{
|
|
name: gobot.DefaultName("MPU6050"),
|
|
connector: a,
|
|
Config: NewConfig(),
|
|
Eventer: gobot.NewEventer(),
|
|
}
|
|
|
|
for _, option := range options {
|
|
option(m)
|
|
}
|
|
|
|
// TODO: add commands to API
|
|
return m
|
|
}
|
|
|
|
// Name returns the name of the device.
|
|
func (h *MPU6050Driver) Name() string { return h.name }
|
|
|
|
// SetName sets the name of the device.
|
|
func (h *MPU6050Driver) SetName(n string) { h.name = n }
|
|
|
|
// Connection returns the connection for the device.
|
|
func (h *MPU6050Driver) Connection() gobot.Connection { return h.connector.(gobot.Connection) }
|
|
|
|
// Start writes initialization bytes to sensor
|
|
func (h *MPU6050Driver) Start() (err error) {
|
|
if err := h.initialize(); err != nil {
|
|
return err
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
// Halt returns true if devices is halted successfully
|
|
func (h *MPU6050Driver) Halt() (err error) { return }
|
|
|
|
// GetData fetches the latest data from the MPU6050
|
|
func (h *MPU6050Driver) GetData() (err error) {
|
|
if _, err = h.connection.Write([]byte{MPU6050_RA_ACCEL_XOUT_H}); err != nil {
|
|
return
|
|
}
|
|
|
|
data := make([]byte, 14)
|
|
_, err = h.connection.Read(data)
|
|
if err != nil {
|
|
return
|
|
}
|
|
|
|
buf := bytes.NewBuffer(data)
|
|
binary.Read(buf, binary.BigEndian, &h.Accelerometer)
|
|
binary.Read(buf, binary.BigEndian, &h.Temperature)
|
|
binary.Read(buf, binary.BigEndian, &h.Gyroscope)
|
|
h.convertToCelsius()
|
|
return
|
|
}
|
|
|
|
func (h *MPU6050Driver) initialize() (err error) {
|
|
bus := h.GetBusOrDefault(h.connector.GetDefaultBus())
|
|
address := h.GetAddressOrDefault(mpu6050Address)
|
|
|
|
h.connection, err = h.connector.GetConnection(address, bus)
|
|
if err != nil {
|
|
return err
|
|
}
|
|
|
|
// setClockSource
|
|
if _, err = h.connection.Write([]byte{MPU6050_RA_PWR_MGMT_1,
|
|
MPU6050_PWR1_CLKSEL_BIT,
|
|
MPU6050_PWR1_CLKSEL_LENGTH,
|
|
MPU6050_CLOCK_PLL_XGYRO}); err != nil {
|
|
return
|
|
}
|
|
|
|
// setFullScaleGyroRange
|
|
if _, err = h.connection.Write([]byte{MPU6050_RA_GYRO_CONFIG,
|
|
MPU6050_GCONFIG_FS_SEL_BIT,
|
|
MPU6050_GCONFIG_FS_SEL_LENGTH,
|
|
MPU6050_GYRO_FS_250}); err != nil {
|
|
return
|
|
}
|
|
|
|
// setFullScaleAccelRange
|
|
if _, err = h.connection.Write([]byte{MPU6050_RA_ACCEL_CONFIG,
|
|
MPU6050_ACONFIG_AFS_SEL_BIT,
|
|
MPU6050_ACONFIG_AFS_SEL_LENGTH,
|
|
MPU6050_ACCEL_FS_2}); err != nil {
|
|
return
|
|
}
|
|
|
|
// setSleepEnabled
|
|
if _, err = h.connection.Write([]byte{MPU6050_RA_PWR_MGMT_1,
|
|
MPU6050_PWR1_ENABLE_BIT,
|
|
0}); err != nil {
|
|
return
|
|
}
|
|
|
|
return nil
|
|
}
|
|
|
|
// The temperature sensor is -40 to +85 degrees Celsius.
|
|
// It is a signed integer.
|
|
// According to the datasheet:
|
|
// 340 per degrees Celsius, -512 at 35 degrees.
|
|
// At 0 degrees: -512 - (340 * 35) = -12412
|
|
func (h *MPU6050Driver) convertToCelsius() {
|
|
h.Temperature = (h.Temperature + 12412) / 340
|
|
}
|