137 lines
3.6 KiB
Go
137 lines
3.6 KiB
Go
package i2c
|
|
|
|
import (
|
|
"bytes"
|
|
"encoding/binary"
|
|
|
|
"gobot.io/x/gobot"
|
|
)
|
|
|
|
const l3gd20hAddress = 0x6B
|
|
|
|
// Control Register 1
|
|
const l3gd20hRegisterCtl1 = 0x20
|
|
const l3gd20hNormalMode = 0x8
|
|
const l3gd20hEnableZ = 0x04
|
|
const l3gd20hEnableY = 0x02
|
|
const l3gd20hEnableX = 0x01
|
|
|
|
// Control Register 4
|
|
const l3gd20hRegisterCtl4 = 0x23
|
|
|
|
const l3gd20hRegisterOutXLSB = 0x28 | 0x80 // set auto-increment bit.
|
|
|
|
// L3GD20HDriver is the gobot driver for the Adafruit Triple-Axis Gyroscope L3GD20H.
|
|
// Device datasheet: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/DM00036465.pdf
|
|
type L3GD20HDriver struct {
|
|
name string
|
|
connection I2c
|
|
scale L3GD20HScale
|
|
}
|
|
|
|
// L3GD20HScale is the scale sensitivity of degrees-per-second.
|
|
type L3GD20HScale byte
|
|
|
|
const (
|
|
// L3GD20HScale250dps is the 250 degress-per-second scale.
|
|
L3GD20HScale250dps = 0x00
|
|
// L3GD20HScale500dps is the 500 degress-per-second scale.
|
|
L3GD20HScale500dps = 0x10
|
|
// L3GD20HScale2000dps is the 2000 degress-per-second scale.
|
|
L3GD20HScale2000dps = 0x30
|
|
)
|
|
|
|
// NewL3GD20HDriver creates a new driver with the i2c interface for the L3GD20H device.
|
|
func NewL3GD20HDriver(c I2c, s L3GD20HScale) *L3GD20HDriver {
|
|
return &L3GD20HDriver{
|
|
name: "L3GD20H",
|
|
connection: c,
|
|
scale: s,
|
|
}
|
|
}
|
|
|
|
// Name returns the name of the device.
|
|
func (d *L3GD20HDriver) Name() string {
|
|
return d.name
|
|
}
|
|
|
|
// SetName sets the name of the device.
|
|
func (d *L3GD20HDriver) SetName(name string) {
|
|
d.name = name
|
|
}
|
|
|
|
// Connection returns the connection of the device.
|
|
func (d *L3GD20HDriver) Connection() gobot.Connection {
|
|
return d.connection.(gobot.Connection)
|
|
}
|
|
|
|
// Scale returns the scale sensitivity of the device.
|
|
func (d *L3GD20HDriver) Scale() L3GD20HScale {
|
|
return d.scale
|
|
}
|
|
|
|
// Start initializes the device.
|
|
func (d *L3GD20HDriver) Start() (err error) {
|
|
if err := d.initialization(); err != nil {
|
|
return err
|
|
}
|
|
return nil
|
|
}
|
|
|
|
func (d *L3GD20HDriver) initialization() (err error) {
|
|
if err = d.connection.I2cStart(l3gd20hAddress); err != nil {
|
|
return err
|
|
}
|
|
// reset the gyroscope.
|
|
if err := d.connection.I2cWrite(l3gd20hAddress, []byte{l3gd20hRegisterCtl1, 0x00}); err != nil {
|
|
return err
|
|
}
|
|
// Enable Z, Y and X axis.
|
|
if err := d.connection.I2cWrite(l3gd20hAddress, []byte{l3gd20hRegisterCtl1, l3gd20hNormalMode | l3gd20hEnableZ | l3gd20hEnableY | l3gd20hEnableX}); err != nil {
|
|
return err
|
|
}
|
|
// Set the sensitivity scale.
|
|
if err := d.connection.I2cWrite(l3gd20hAddress, []byte{l3gd20hRegisterCtl4, byte(d.scale)}); err != nil {
|
|
return err
|
|
}
|
|
return nil
|
|
}
|
|
|
|
// Halt halts the device.
|
|
func (d *L3GD20HDriver) Halt() (err error) {
|
|
return nil
|
|
}
|
|
|
|
// XYZ returns the current change in degrees per second, for the 3 axis.
|
|
func (d *L3GD20HDriver) XYZ() (x float32, y float32, z float32, err error) {
|
|
if err := d.connection.I2cWrite(l3gd20hAddress, []byte{l3gd20hRegisterOutXLSB}); err != nil {
|
|
return 0, 0, 0, nil
|
|
}
|
|
var measurements []byte
|
|
if measurements, err = d.connection.I2cRead(l3gd20hAddress, 6); err != nil {
|
|
return 0, 0, 0, nil
|
|
}
|
|
|
|
var rawX int16
|
|
var rawY int16
|
|
var rawZ int16
|
|
buf := bytes.NewBuffer(measurements)
|
|
binary.Read(buf, binary.LittleEndian, &rawX)
|
|
binary.Read(buf, binary.LittleEndian, &rawY)
|
|
binary.Read(buf, binary.LittleEndian, &rawZ)
|
|
|
|
// Values copied from Adafruit's library:
|
|
// https://github.com/adafruit/Adafruit_L3GD20_U/blob/master/Adafruit_L3GD20_U.h
|
|
var factor float32
|
|
switch(d.scale) {
|
|
case L3GD20HScale250dps:
|
|
factor = 0.00875
|
|
case L3GD20HScale500dps:
|
|
factor = 0.0175
|
|
case L3GD20HScale2000dps:
|
|
factor = 0.07
|
|
}
|
|
|
|
return float32(rawX) * factor, float32(rawY) * factor, float32(rawZ) * factor, nil
|
|
}
|