2014-01-02 06:50:57 +05:30
|
|
|
// Package l3gd20 allows interacting with L3GD20 gyroscoping sensor.
|
|
|
|
package l3gd20
|
|
|
|
|
|
|
|
import (
|
|
|
|
"fmt"
|
|
|
|
"log"
|
|
|
|
"math"
|
|
|
|
"sync"
|
|
|
|
"time"
|
|
|
|
|
2014-03-03 00:51:23 +05:30
|
|
|
"github.com/kidoman/embd"
|
2014-01-02 06:50:57 +05:30
|
|
|
)
|
|
|
|
|
|
|
|
const (
|
|
|
|
address = 0x6B
|
|
|
|
id = 0xD4
|
|
|
|
|
|
|
|
dpsToRps = 0.017453293
|
|
|
|
|
|
|
|
whoAmI = 0x0F
|
|
|
|
ctrlReg1 = 0x20
|
|
|
|
ctrlReg2 = 0x21
|
|
|
|
ctrlReg3 = 0x22
|
|
|
|
ctrlReg4 = 0x23
|
|
|
|
ctrlReg5 = 0x24
|
|
|
|
tempData = 0x26
|
|
|
|
statusReg = 0x27
|
|
|
|
|
|
|
|
xlReg = 0x28
|
|
|
|
xhReg = 0x29
|
|
|
|
ylReg = 0x2A
|
|
|
|
yhReg = 0x2B
|
|
|
|
zlReg = 0x2C
|
|
|
|
zhReg = 0x2D
|
|
|
|
|
2014-01-09 05:26:21 +05:30
|
|
|
dr95 = 0x00
|
|
|
|
dr190 = 0x40
|
|
|
|
dr380 = 0x80
|
|
|
|
dr760 = 0xC0
|
2014-01-02 06:50:57 +05:30
|
|
|
|
2014-01-09 05:26:21 +05:30
|
|
|
xEnabled = 0x01
|
|
|
|
yEnabled = 0x02
|
|
|
|
zEnabled = 0x04
|
2014-01-02 06:50:57 +05:30
|
|
|
|
2014-01-09 05:26:21 +05:30
|
|
|
powerOn = 0x08
|
|
|
|
powerOff = 0x00
|
|
|
|
|
|
|
|
ctrlReg1Default = powerOn | xEnabled | yEnabled | zEnabled
|
|
|
|
ctrlReg1Finished = powerOff | xEnabled | yEnabled | zEnabled
|
2014-01-02 06:50:57 +05:30
|
|
|
|
|
|
|
zyxAvailable = 0x08
|
|
|
|
|
2014-01-09 05:26:21 +05:30
|
|
|
odr = 95
|
|
|
|
mult = 1.0 / odr
|
|
|
|
pollDelay = mult * 1000 * 1000
|
2014-01-02 06:50:57 +05:30
|
|
|
)
|
|
|
|
|
|
|
|
// Range represents a L3GD20 range setting.
|
|
|
|
type Range struct {
|
|
|
|
sensitivity float64
|
|
|
|
|
|
|
|
value byte
|
|
|
|
}
|
|
|
|
|
|
|
|
// The three range settings supported by L3GD20.
|
|
|
|
var (
|
|
|
|
R250DPS = &Range{sensitivity: 0.00875, value: 0x00}
|
|
|
|
R500DPS = &Range{sensitivity: 0.0175, value: 0x10}
|
|
|
|
R2000DPS = &Range{sensitivity: 0.070, value: 0x20}
|
|
|
|
)
|
|
|
|
|
|
|
|
type axis struct {
|
|
|
|
name string
|
|
|
|
|
|
|
|
lowReg, highReg byte
|
|
|
|
|
|
|
|
availableMask byte
|
|
|
|
}
|
|
|
|
|
|
|
|
func (a *axis) regs() (byte, byte) {
|
|
|
|
return a.lowReg, a.highReg
|
|
|
|
}
|
|
|
|
|
|
|
|
func (a axis) String() string {
|
|
|
|
return a.name
|
|
|
|
}
|
|
|
|
|
|
|
|
var (
|
|
|
|
ax = &axis{name: "X", lowReg: xlReg, highReg: xhReg, availableMask: 0x01}
|
|
|
|
ay = &axis{name: "Y", lowReg: ylReg, highReg: yhReg, availableMask: 0x02}
|
|
|
|
az = &axis{name: "Z", lowReg: zlReg, highReg: zhReg, availableMask: 0x04}
|
|
|
|
)
|
|
|
|
|
|
|
|
type axisCalibration struct {
|
|
|
|
min, max, mean float64
|
|
|
|
}
|
|
|
|
|
|
|
|
func (ac axisCalibration) adjust(value float64) float64 {
|
|
|
|
if value >= ac.min && value <= ac.max {
|
|
|
|
return 0
|
|
|
|
}
|
|
|
|
return value - ac.mean
|
|
|
|
}
|
|
|
|
|
|
|
|
func (ac axisCalibration) String() string {
|
|
|
|
return fmt.Sprintf("%v, %v, %v", ac.min, ac.max, ac.mean)
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
type Orientation struct {
|
|
|
|
X, Y, Z float64
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
// L3GD20 represents a L3GD20 3-axis gyroscope.
|
|
|
|
type L3GD20 struct {
|
2014-03-03 00:51:23 +05:30
|
|
|
Bus embd.I2CBus
|
2014-01-07 22:11:41 +05:30
|
|
|
Range *Range
|
2014-01-02 06:50:57 +05:30
|
|
|
|
|
|
|
initialized bool
|
|
|
|
mu sync.RWMutex
|
|
|
|
|
|
|
|
xac, yac, zac axisCalibration
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
orientations chan Orientation
|
|
|
|
closing chan chan struct{}
|
2014-01-02 06:50:57 +05:30
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
Debug bool
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
|
|
|
|
|
|
|
// New creates a new L3GD20 interface. The bus variable controls
|
|
|
|
// the I2C bus used to communicate with the device.
|
2014-03-03 00:51:23 +05:30
|
|
|
func New(bus embd.I2CBus, Range *Range) *L3GD20 {
|
2014-01-07 22:11:41 +05:30
|
|
|
return &L3GD20{
|
|
|
|
Bus: bus,
|
|
|
|
Range: Range,
|
|
|
|
Debug: false,
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
type values []float64
|
|
|
|
|
|
|
|
func (vs values) min() float64 {
|
|
|
|
value := math.MaxFloat64
|
|
|
|
for _, v := range vs {
|
|
|
|
value = math.Min(value, v)
|
|
|
|
}
|
|
|
|
return value
|
|
|
|
}
|
|
|
|
|
|
|
|
func (vs values) max() float64 {
|
|
|
|
value := -math.MaxFloat64
|
|
|
|
for _, v := range vs {
|
|
|
|
value = math.Max(value, v)
|
|
|
|
}
|
|
|
|
return value
|
|
|
|
}
|
|
|
|
|
|
|
|
func (vs values) mean() float64 {
|
|
|
|
sum := 0.0
|
|
|
|
for _, v := range vs {
|
|
|
|
sum += v
|
|
|
|
}
|
|
|
|
return sum / float64(len(vs))
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) calibrate(a *axis) (ac axisCalibration, err error) {
|
|
|
|
if d.Debug {
|
2014-01-02 06:50:57 +05:30
|
|
|
log.Printf("l3gd20: calibrating %v axis", a)
|
|
|
|
}
|
|
|
|
|
|
|
|
values := make(values, 0)
|
|
|
|
for i := 0; i < 20; i++ {
|
|
|
|
again:
|
|
|
|
var available bool
|
|
|
|
if available, err = d.axisStatus(a); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
if !available {
|
|
|
|
time.Sleep(100 * time.Microsecond)
|
|
|
|
goto again
|
|
|
|
}
|
|
|
|
var value float64
|
2014-01-07 22:11:41 +05:30
|
|
|
if value, err = d.readOrientationDelta(a); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
values = append(values, value)
|
|
|
|
}
|
|
|
|
ac.min, ac.max, ac.mean = values.min(), values.max(), values.mean()
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
if d.Debug {
|
2014-01-02 06:50:57 +05:30
|
|
|
log.Printf("l3gd20: %v axis calibration (%v)", a, ac)
|
|
|
|
}
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) setup() (err error) {
|
2014-01-02 06:50:57 +05:30
|
|
|
d.mu.RLock()
|
|
|
|
if d.initialized {
|
|
|
|
d.mu.RUnlock()
|
|
|
|
return
|
|
|
|
}
|
|
|
|
d.mu.RUnlock()
|
|
|
|
|
|
|
|
d.mu.Lock()
|
|
|
|
defer d.mu.Unlock()
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
d.orientations = make(chan Orientation)
|
|
|
|
|
|
|
|
if err = d.Bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Default); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
2014-01-07 22:11:41 +05:30
|
|
|
if err = d.Bus.WriteByteToReg(address, ctrlReg4, d.Range.value); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
// Calibrate
|
|
|
|
if d.xac, err = d.calibrate(ax); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
if d.yac, err = d.calibrate(ay); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
if d.zac, err = d.calibrate(az); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
d.initialized = true
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) axisStatus(a *axis) (available bool, err error) {
|
2014-01-02 06:50:57 +05:30
|
|
|
var data byte
|
2014-01-07 22:11:41 +05:30
|
|
|
if data, err = d.Bus.ReadByteFromReg(address, statusReg); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
if data&zyxAvailable == 0 {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
available = data&a.availableMask != 0
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) readOrientationDelta(a *axis) (value float64, err error) {
|
2014-01-02 06:50:57 +05:30
|
|
|
rl, rh := a.regs()
|
|
|
|
var l, h byte
|
2014-01-07 22:11:41 +05:30
|
|
|
if l, err = d.Bus.ReadByteFromReg(address, rl); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
2014-01-07 22:11:41 +05:30
|
|
|
if h, err = d.Bus.ReadByteFromReg(address, rh); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
value = float64(int16(h)<<8 | int16(l))
|
2014-01-09 00:55:43 +05:30
|
|
|
value *= d.Range.sensitivity
|
2014-01-02 06:50:57 +05:30
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) calibratedOrientationDelta(a *axis) (value float64, err error) {
|
|
|
|
if value, err = d.readOrientationDelta(a); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
switch a {
|
|
|
|
case ax:
|
|
|
|
value = d.xac.adjust(value)
|
|
|
|
case ay:
|
|
|
|
value = d.yac.adjust(value)
|
|
|
|
case az:
|
|
|
|
value = d.zac.adjust(value)
|
|
|
|
}
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) measureOrientationDelta() (dx, dy, dz float64, err error) {
|
2014-01-02 06:50:57 +05:30
|
|
|
if err = d.setup(); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
if dx, err = d.calibratedOrientationDelta(ax); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
2014-01-07 22:11:41 +05:30
|
|
|
if dy, err = d.calibratedOrientationDelta(ay); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
2014-01-07 22:11:41 +05:30
|
|
|
if dz, err = d.calibratedOrientationDelta(az); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
// Orientation returns the current orientation reading.
|
|
|
|
func (d *L3GD20) OrientationDelta() (dx, dy, dz float64, err error) {
|
|
|
|
return d.measureOrientationDelta()
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
// Temperature returns the current temperature reading.
|
|
|
|
func (d *L3GD20) Temperature() (temp int, err error) {
|
2014-01-02 06:50:57 +05:30
|
|
|
if err = d.setup(); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
var data byte
|
2014-01-07 22:11:41 +05:30
|
|
|
if data, err = d.Bus.ReadByteFromReg(address, tempData); err != nil {
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
temp = int(int8(data))
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-03-23 15:47:58 +05:30
|
|
|
// Orientations returns a channel which will have the current temperature reading.
|
2014-01-07 22:11:41 +05:30
|
|
|
func (d *L3GD20) Orientations() (orientations <-chan Orientation, err error) {
|
|
|
|
if err = d.setup(); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
orientations = d.orientations
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
2014-01-02 06:50:57 +05:30
|
|
|
|
2014-01-07 22:11:41 +05:30
|
|
|
// Start starts the data acquisition loop.
|
|
|
|
func (d *L3GD20) Start() (err error) {
|
|
|
|
if err = d.setup(); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
d.closing = make(chan chan struct{})
|
|
|
|
|
|
|
|
go func() {
|
|
|
|
var x, y, z float64
|
|
|
|
var orientations chan Orientation
|
2014-01-02 06:50:57 +05:30
|
|
|
|
2014-01-09 05:26:21 +05:30
|
|
|
timer := time.Tick(time.Duration(math.Floor(pollDelay)) * time.Microsecond)
|
2014-01-02 06:50:57 +05:30
|
|
|
|
|
|
|
for {
|
|
|
|
select {
|
2014-01-09 05:26:21 +05:30
|
|
|
case <-timer:
|
2014-01-07 22:11:41 +05:30
|
|
|
dx, dy, dz, err := d.measureOrientationDelta()
|
|
|
|
if err != nil {
|
|
|
|
log.Printf("l3gd20: %v", err)
|
|
|
|
} else {
|
|
|
|
x += dx * mult
|
|
|
|
y += dy * mult
|
|
|
|
z += dz * mult
|
|
|
|
orientations = d.orientations
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
2014-01-07 22:11:41 +05:30
|
|
|
case orientations <- Orientation{x, y, z}:
|
2014-01-08 19:44:56 +05:30
|
|
|
orientations = nil
|
2014-01-07 22:11:41 +05:30
|
|
|
case waitc := <-d.closing:
|
|
|
|
waitc <- struct{}{}
|
|
|
|
close(d.orientations)
|
2014-01-02 06:50:57 +05:30
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}()
|
|
|
|
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
2014-03-23 15:47:58 +05:30
|
|
|
// Stop the data acquisition loop.
|
2014-01-08 19:43:56 +05:30
|
|
|
func (d *L3GD20) Stop() (err error) {
|
2014-01-07 22:11:41 +05:30
|
|
|
if d.closing != nil {
|
|
|
|
waitc := make(chan struct{})
|
|
|
|
d.closing <- waitc
|
|
|
|
<-waitc
|
2014-01-09 00:56:44 +05:30
|
|
|
d.closing = nil
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|
2014-01-08 19:43:56 +05:30
|
|
|
if err = d.Bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Finished); err != nil {
|
|
|
|
return
|
|
|
|
}
|
|
|
|
d.initialized = false
|
|
|
|
return
|
|
|
|
}
|
|
|
|
|
|
|
|
// Close.
|
|
|
|
func (d *L3GD20) Close() (err error) {
|
|
|
|
return d.Stop()
|
2014-01-02 06:50:57 +05:30
|
|
|
}
|