From 95b06553e5b864f2bcf78d946988d55eb8c4e2ff Mon Sep 17 00:00:00 2001
From: Karan Misra <kidoman@gmail.com>
Date: Thu, 9 Jan 2014 05:26:21 +0530
Subject: [PATCH] l3gd20: clean up

---
 samples/l3gd20.go       |  8 ++++++--
 sensor/l3gd20/l3gd20.go | 42 +++++++++++++++++------------------------
 2 files changed, 23 insertions(+), 27 deletions(-)

diff --git a/samples/l3gd20.go b/samples/l3gd20.go
index 3576f63..3f3061a 100644
--- a/samples/l3gd20.go
+++ b/samples/l3gd20.go
@@ -4,6 +4,7 @@ import (
 	"log"
 	"os"
 	"os/signal"
+	"time"
 
 	"github.com/kid0m4n/go-rpi/i2c"
 	"github.com/kid0m4n/go-rpi/sensor/l3gd20"
@@ -15,7 +16,7 @@ func main() {
 		log.Panic(err)
 	}
 	gyro := l3gd20.New(bus, l3gd20.R250DPS)
-	gyro.Poll = 50
+	gyro.Debug = true
 	defer gyro.Close()
 
 	gyro.Start()
@@ -28,9 +29,12 @@ func main() {
 		log.Panic(err)
 	}
 
+	timer := time.Tick(250 * time.Millisecond)
+
 	for {
 		select {
-		case orientation := <-orientations:
+		case <-timer:
+			orientation := <-orientations
 			log.Printf("x: %v, y: %v, z: %v", orientation.X, orientation.Y, orientation.Z)
 		case <-quit:
 			return
diff --git a/sensor/l3gd20/l3gd20.go b/sensor/l3gd20/l3gd20.go
index d7f3c1e..e267ab3 100644
--- a/sensor/l3gd20/l3gd20.go
+++ b/sensor/l3gd20/l3gd20.go
@@ -33,22 +33,26 @@ const (
 	zlReg = 0x2C
 	zhReg = 0x2D
 
-	xEnabled  = 0x01
-	xDisabled = 0x00
-	yEnabled  = 0x02
-	yDisabled = 0x00
-	zEnabled  = 0x04
-	zDisabled = 0x00
+	dr95  = 0x00
+	dr190 = 0x40
+	dr380 = 0x80
+	dr760 = 0xC0
 
-	powerOn   = 0x08
-	powerDown = 0x00
+	xEnabled = 0x01
+	yEnabled = 0x02
+	zEnabled = 0x04
 
-	ctrlReg1Default  = xEnabled | yEnabled | zEnabled | powerOn
-	ctrlReg1Finished = xDisabled | yDisabled | zDisabled | powerDown
+	powerOn  = 0x08
+	powerOff = 0x00
+
+	ctrlReg1Default  = powerOn | xEnabled | yEnabled | zEnabled
+	ctrlReg1Finished = powerOff | xEnabled | yEnabled | zEnabled
 
 	zyxAvailable = 0x08
 
-	pollDelay = 100
+	odr       = 95
+	mult      = 1.0 / odr
+	pollDelay = mult * 1000 * 1000
 )
 
 // Range represents a L3GD20 range setting.
@@ -111,8 +115,6 @@ type L3GD20 struct {
 	Bus   i2c.Bus
 	Range *Range
 
-	Poll int
-
 	initialized bool
 	mu          sync.RWMutex
 
@@ -130,7 +132,6 @@ func New(bus i2c.Bus, Range *Range) *L3GD20 {
 	return &L3GD20{
 		Bus:   bus,
 		Range: Range,
-		Poll:  pollDelay,
 		Debug: false,
 	}
 }
@@ -336,30 +337,21 @@ func (d *L3GD20) Start() (err error) {
 	go func() {
 		var x, y, z float64
 		var orientations chan Orientation
-		oldTime := time.Now()
 
-		var timer <-chan time.Time
-		resetTimer := func() {
-			timer = time.After(time.Duration(d.Poll) * time.Millisecond)
-		}
-		resetTimer()
+		timer := time.Tick(time.Duration(math.Floor(pollDelay)) * time.Microsecond)
 
 		for {
 			select {
-			case currTime := <-timer:
+			case <-timer:
 				dx, dy, dz, err := d.measureOrientationDelta()
 				if err != nil {
 					log.Printf("l3gd20: %v", err)
 				} else {
-					timeElapsed := currTime.Sub(oldTime)
-					mult := timeElapsed.Seconds()
 					x += dx * mult
 					y += dy * mult
 					z += dz * mult
 					orientations = d.orientations
 				}
-				oldTime = currTime
-				resetTimer()
 			case orientations <- Orientation{x, y, z}:
 				orientations = nil
 			case waitc := <-d.closing: