From e5707501d09f5bf4af26e27144181c2ef5c88571 Mon Sep 17 00:00:00 2001
From: deadprogram <ron@hybridgroup.com>
Date: Tue, 16 Feb 2016 17:55:07 -0800
Subject: [PATCH] Fix #236 & fix #239 by correcting initialization and
 temperature conversion for MPU-6050

---
 examples/chip_mpu6050.go        | 35 +++++++++++++++++++++++++++++++++
 platforms/i2c/mpu6050_driver.go | 27 +++++++++++++++++--------
 2 files changed, 54 insertions(+), 8 deletions(-)
 create mode 100644 examples/chip_mpu6050.go

diff --git a/examples/chip_mpu6050.go b/examples/chip_mpu6050.go
new file mode 100644
index 000000000..1e0605e70
--- /dev/null
+++ b/examples/chip_mpu6050.go
@@ -0,0 +1,35 @@
+package main
+
+import (
+	"fmt"
+	"time"
+
+	"github.com/hybridgroup/gobot"
+	"github.com/hybridgroup/gobot/platforms/i2c"
+  "github.com/hybridgroup/gobot/platforms/chip"
+)
+
+func main() {
+	gbot := gobot.NewGobot()
+
+	board := chip.NewChipAdaptor("chip")
+	mpu6050 := i2c.NewMPU6050Driver(board, "mpu6050")
+
+	work := func() {
+		gobot.Every(100*time.Millisecond, func() {
+			fmt.Println("Accelerometer", mpu6050.Accelerometer)
+			fmt.Println("Gyroscope", mpu6050.Gyroscope)
+			fmt.Println("Temperature", mpu6050.Temperature)
+		})
+	}
+
+	robot := gobot.NewRobot("mpu6050Bot",
+		[]gobot.Connection{board},
+		[]gobot.Device{mpu6050},
+		work,
+	)
+
+	gbot.AddRobot(robot)
+
+	gbot.Start()
+}
diff --git a/platforms/i2c/mpu6050_driver.go b/platforms/i2c/mpu6050_driver.go
index faf79d2c6..c456cb0ab 100644
--- a/platforms/i2c/mpu6050_driver.go
+++ b/platforms/i2c/mpu6050_driver.go
@@ -26,6 +26,7 @@ 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
@@ -84,8 +85,9 @@ func (h *MPU6050Driver) Start() (errs []error) {
 			}
 			buf := bytes.NewBuffer(ret)
 			binary.Read(buf, binary.BigEndian, &h.Accelerometer)
-			binary.Read(buf, binary.BigEndian, &h.Gyroscope)
 			binary.Read(buf, binary.BigEndian, &h.Temperature)
+			binary.Read(buf, binary.BigEndian, &h.Gyroscope)
+			h.convertToCelsius()
 			<-time.After(h.interval)
 		}
 	}()
@@ -109,12 +111,12 @@ func (h *MPU6050Driver) initialize() (err error) {
 	}
 
 	// setFullScaleGyroRange
-	if err = h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_GYRO_FS_250,
-		MPU6050_RA_GYRO_CONFIG,
-		MPU6050_GCONFIG_FS_SEL_LENGTH,
-		MPU6050_GCONFIG_FS_SEL_BIT}); err != nil {
-		return
-	}
+  if err = h.connection.I2cWrite(mpu6050Address, []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.I2cWrite(mpu6050Address, []byte{MPU6050_RA_ACCEL_CONFIG,
@@ -126,10 +128,19 @@ func (h *MPU6050Driver) initialize() (err error) {
 
 	// setSleepEnabled
 	if err = h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_RA_PWR_MGMT_1,
-		MPU6050_PWR1_SLEEP_BIT,
+		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
+}