go-embedded/embd
go-embedded
/
embd
mirror of https://github.com/kidoman/embd
1
0
Fork 0
Code Issues Releases Wiki Activity

l3gd20: calculate absolute values and provide it via a channel

This commit is contained in:
Karan Misra 2014-01-07 22:11:41 +05:30
parent 58a1eb5a56
commit 3d71941214
2 changed files with 104 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
samples/l3gd20.go
View File

@ -2,7 +2,8 @@ package main
import ( import (
"log" "log"
"time" "os"
"os/signal"
"github.com/kid0m4n/go-rpi/i2c" "github.com/kid0m4n/go-rpi/i2c"
"github.com/kid0m4n/go-rpi/sensor/l3gd20" "github.com/kid0m4n/go-rpi/sensor/l3gd20"
@ -16,21 +17,22 @@ func main() {
gyro := l3gd20.New(bus, l3gd20.R250DPS) gyro := l3gd20.New(bus, l3gd20.R250DPS)
defer gyro.Close() defer gyro.Close()
x, y, z := 0.0, 0.0, 0.0 gyro.Start()
dt := 0.1 // Seconds
quit := make(chan os.Signal, 1)
signal.Notify(quit, os.Interrupt, os.Kill)
orientations, err := gyro.Orientations()
if err != nil {
log.Panic(err)
}
for { for {
dx, dy, dz, err := gyro.OrientationDelta() select {
if err != nil { case orientation := <-orientations:
log.Panic(err) log.Printf("x: %v, y: %v, z: %v", orientation.X, orientation.Y, orientation.Z)
case <-quit:
return
} }
x += dx * dt
y += dy * dt
z += dz * dt
log.Printf("%v", z)
time.Sleep(time.Duration(dt*1000) * time.Millisecond)
} }
} }

169
sensor/l3gd20/l3gd20.go
View File

@ -87,17 +87,6 @@ var (
az = &axis{name: "Z", lowReg: zlReg, highReg: zhReg, availableMask: 0x04} az = &axis{name: "Z", lowReg: zlReg, highReg: zhReg, availableMask: 0x04}
) )
// A L3GD20 implements access to the L3GD20 sensor.
type L3GD20 interface {
// Orientation returns the current orientation reading.
OrientationDelta() (x, y, z float64, err error)
// Temperature returns the current temperature reading.
Temperature() (temp int, err error)
// Close.
Close() error
}
type axisCalibration struct { type axisCalibration struct {
min, max, mean float64 min, max, mean float64
} }
@ -113,35 +102,36 @@ func (ac axisCalibration) String() string {
return fmt.Sprintf("%v, %v, %v", ac.min, ac.max, ac.mean) return fmt.Sprintf("%v, %v, %v", ac.min, ac.max, ac.mean)
} }
type data struct { type Orientation struct {
dx, dy, dz float64 X, Y, Z float64
} }
type l3gd20 struct { // L3GD20 represents a L3GD20 3-axis gyroscope.
bus i2c.Bus type L3GD20 struct {
rng *Range Bus i2c.Bus
Range *Range
poll int Poll int
initialized bool initialized bool
mu sync.RWMutex mu sync.RWMutex
xac, yac, zac axisCalibration xac, yac, zac axisCalibration
orientations chan data orientations chan Orientation
quit chan struct{} closing chan chan struct{}
debug bool Debug bool
} }
// New creates a new L3GD20 interface. The bus variable controls // New creates a new L3GD20 interface. The bus variable controls
// the I2C bus used to communicate with the device. // the I2C bus used to communicate with the device.
func New(bus i2c.Bus, rng *Range) L3GD20 { func New(bus i2c.Bus, Range *Range) *L3GD20 {
return &l3gd20{ return &L3GD20{
bus: bus, Bus: bus,
rng: rng, Range: Range,
poll: pollDelay, Poll: pollDelay,
debug: false, Debug: false,
} }
} }
@ -171,8 +161,8 @@ func (vs values) mean() float64 {
return sum / float64(len(vs)) return sum / float64(len(vs))
} }
func (d *l3gd20) calibrate(a *axis) (ac axisCalibration, err error) { func (d *L3GD20) calibrate(a *axis) (ac axisCalibration, err error) {
if d.debug { if d.Debug {
log.Printf("l3gd20: calibrating %v axis", a) log.Printf("l3gd20: calibrating %v axis", a)
} }
@ -188,21 +178,21 @@ func (d *l3gd20) calibrate(a *axis) (ac axisCalibration, err error) {
goto again goto again
} }
var value float64 var value float64
if value, err = d.readOrientation(a); err != nil { if value, err = d.readOrientationDelta(a); err != nil {
return return
} }
values = append(values, value) values = append(values, value)
} }
ac.min, ac.max, ac.mean = values.min(), values.max(), values.mean() ac.min, ac.max, ac.mean = values.min(), values.max(), values.mean()
if d.debug { if d.Debug {
log.Printf("l3gd20: %v axis calibration (%v)", a, ac) log.Printf("l3gd20: %v axis calibration (%v)", a, ac)
} }
return return
} }
func (d *l3gd20) setup() (err error) { func (d *L3GD20) setup() (err error) {
d.mu.RLock() d.mu.RLock()
if d.initialized { if d.initialized {
d.mu.RUnlock() d.mu.RUnlock()
@ -213,10 +203,12 @@ func (d *l3gd20) setup() (err error) {
d.mu.Lock() d.mu.Lock()
defer d.mu.Unlock() defer d.mu.Unlock()
if err = d.bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Default); err != nil { d.orientations = make(chan Orientation)
if err = d.Bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Default); err != nil {
return return
} }
if err = d.bus.WriteByteToReg(address, ctrlReg4, d.rng.value); err != nil { if err = d.Bus.WriteByteToReg(address, ctrlReg4, d.Range.value); err != nil {
return return
} }
@ -236,13 +228,9 @@ func (d *l3gd20) setup() (err error) {
return return
} }
func (d *l3gd20) SetPollDelay(delay int) { func (d *L3GD20) axisStatus(a *axis) (available bool, err error) {
d.poll = delay
}
func (d *l3gd20) axisStatus(a *axis) (available bool, err error) {
var data byte var data byte
if data, err = d.bus.ReadByteFromReg(address, statusReg); err != nil { if data, err = d.Bus.ReadByteFromReg(address, statusReg); err != nil {
return return
} }
@ -255,26 +243,26 @@ func (d *l3gd20) axisStatus(a *axis) (available bool, err error) {
return return
} }
func (d *l3gd20) readOrientation(a *axis) (value float64, err error) { func (d *L3GD20) readOrientationDelta(a *axis) (value float64, err error) {
rl, rh := a.regs() rl, rh := a.regs()
var l, h byte var l, h byte
if l, err = d.bus.ReadByteFromReg(address, rl); err != nil { if l, err = d.Bus.ReadByteFromReg(address, rl); err != nil {
return return
} }
if h, err = d.bus.ReadByteFromReg(address, rh); err != nil { if h, err = d.Bus.ReadByteFromReg(address, rh); err != nil {
return return
} }
value = float64(int16(h)<<8 | int16(l)) value = float64(int16(h)<<8 | int16(l))
sensitivity := d.rng.sensitivity sensitivity := d.Range.sensitivity
value *= sensitivity value *= sensitivity
return return
} }
func (d *l3gd20) calibratedOrientation(a *axis) (value float64, err error) { func (d *L3GD20) calibratedOrientationDelta(a *axis) (value float64, err error) {
if value, err = d.readOrientation(a); err != nil { if value, err = d.readOrientationDelta(a); err != nil {
return return
} }
@ -290,46 +278,37 @@ func (d *l3gd20) calibratedOrientation(a *axis) (value float64, err error) {
return return
} }
func (d *l3gd20) measureOrientation() (dx, dy, dz float64, err error) { func (d *L3GD20) measureOrientationDelta() (dx, dy, dz float64, err error) {
if err = d.setup(); err != nil { if err = d.setup(); err != nil {
return return
} }
if dx, err = d.calibratedOrientation(ax); err != nil { if dx, err = d.calibratedOrientationDelta(ax); err != nil {
return return
} }
if dy, err = d.calibratedOrientation(ay); err != nil { if dy, err = d.calibratedOrientationDelta(ay); err != nil {
return return
} }
if dz, err = d.calibratedOrientation(az); err != nil { if dz, err = d.calibratedOrientationDelta(az); err != nil {
return return
} }
return return
} }
func (d *l3gd20) OrientationDelta() (dx, dy, dz float64, err error) { // Orientation returns the current orientation reading.
select { func (d *L3GD20) OrientationDelta() (dx, dy, dz float64, err error) {
case data := <-d.orientations: return d.measureOrientationDelta()
dx, dy, dz = data.dx, data.dy, data.dz
return
default:
if d.debug {
log.Printf("l3gd20: no orientation available... measuring")
}
return d.measureOrientation()
}
panic("cannot reach here")
} }
func (d *l3gd20) Temperature() (temp int, err error) { // Temperature returns the current temperature reading.
func (d *L3GD20) Temperature() (temp int, err error) {
if err = d.setup(); err != nil { if err = d.setup(); err != nil {
return return
} }
var data byte var data byte
if data, err = d.bus.ReadByteFromReg(address, tempData); err != nil { if data, err = d.Bus.ReadByteFromReg(address, tempData); err != nil {
return return
} }
@ -338,25 +317,50 @@ func (d *l3gd20) Temperature() (temp int, err error) {
return return
} }
func (d *l3gd20) Run() (err error) { func (d *L3GD20) Orientations() (orientations <-chan Orientation, err error) {
if err = d.setup(); err != nil {
return
}
orientations = d.orientations
return
}
// 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() { go func() {
d.quit = make(chan struct{}) var x, y, z float64
var orientations chan Orientation
oldTime := time.Now()
timer := time.Tick(time.Duration(d.poll) * time.Millisecond) timer := time.Tick(time.Duration(d.Poll) * time.Millisecond)
var dt data
for { for {
select { select {
case <-timer: case currTime := <-timer:
var err error dx, dy, dz, err := d.measureOrientationDelta()
dt.dx, dt.dy, dt.dz, err = d.measureOrientation() if err != nil {
if err == nil && d.orientations == nil { log.Printf("l3gd20: %v", err)
d.orientations = make(chan data) } else {
timeElapsed := currTime.Sub(oldTime)
mult := timeElapsed.Seconds()
x += dx * mult
y += dy * mult
z += dz * mult
orientations = d.orientations
} }
case d.orientations <- dt: oldTime = currTime
case <-d.quit: case orientations <- Orientation{x, y, z}:
d.orientations = nil case waitc := <-d.closing:
waitc <- struct{}{}
close(d.orientations)
return return
} }
@ -366,9 +370,12 @@ func (d *l3gd20) Run() (err error) {
return return
} }
func (d *l3gd20) Close() (err error) { // Close.
if d.quit != nil { func (d *L3GD20) Close() (err error) {
d.quit <- struct{}{} if d.closing != nil {
waitc := make(chan struct{})
d.closing <- waitc
<-waitc
} }
return d.bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Finished) return d.Bus.WriteByteToReg(address, ctrlReg1, ctrlReg1Finished)
} }