From 8d88e4f91c376c3dc6af28fba53a5d9753b7b0f0 Mon Sep 17 00:00:00 2001
From: Christopher Young <cyoung@gmail.com>
Date: Fri, 21 Aug 2015 04:12:21 -0400
Subject: [PATCH] Testing heading functions for MPU6050.

---
 mpu6050/mpu6050.go | 63 +++++++++++++++++++++++++++++++++++++---------
 1 file changed, 51 insertions(+), 12 deletions(-)

diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go
index 7b3f9c33..61b31a3e 100644
--- a/mpu6050/mpu6050.go
+++ b/mpu6050/mpu6050.go
@@ -24,6 +24,9 @@ const (
 
 	PWR_MGMT_1 = 0x6B
 
+	GYRO_CONFIG = 0x1B
+	ACCEL_CONFIG = 0x1C
+
 	ACCEL_SCALE = 16384.0 // Assume AFS_SEL = 0.
 	GYRO_SCALE  = 131.0   // Assume FS_SEL = 0.
 
@@ -53,8 +56,9 @@ type MPU6050 struct {
 	pitch_resting float64
 	roll_resting  float64
 
-	pitch float64
-	roll  float64
+	pitch   float64
+	roll    float64
+	heading float64
 	//	gyro		chan XYZ
 	//	accel		chan XYZ
 
@@ -72,6 +76,9 @@ func New(bus embd.I2CBus) *MPU6050 {
 func (d *MPU6050) StartUp() error {
 	d.Bus.WriteByteToReg(address, PWR_MGMT_1, 0) // Wake device up.
 
+	d.Bus.WriteByteToReg(address, GYRO_CONFIG, 0) // FS_SEL = 0
+	d.Bus.WriteByteToReg(address, ACCEL_CONFIG, 0) // AFS_SEL = 0
+
 	d.pitch_history = make([]float64, 0)
 	d.roll_history = make([]float64, 0)
 
@@ -153,10 +160,10 @@ func (d *MPU6050) calculatePitchAndRoll() {
 	// Accel.
 
 	p1 := math.Atan2(float64(accel.y), dist(accel.x, accel.z))
-	p1_deg := p1 * (180 / math.Pi)
+	p1_deg := p1 * (180.0 / math.Pi)
 
 	r1 := math.Atan2(float64(accel.x), dist(accel.y, accel.z))
-	r1_deg := -r1 * (180 / math.Pi)
+	r1_deg := -r1 * (180.0 / math.Pi)
 
 	// Gyro.
 
@@ -172,24 +179,46 @@ func (d *MPU6050) calculatePitchAndRoll() {
 	d.pitch_history = append(d.pitch_history, d.pitch)
 	d.roll_history = append(d.roll_history, d.roll)
 
+	//FIXME: Experimental (heading).
+
+
+
+	f := math.Atan2(float64(d.gyro_reading.z), dist(float32(d.pitch), float32(d.roll)))
+	h1_deg := -float64(3.42857142857)*f * (180.0 / math.Pi)
+	//	d.heading = float64((float64(3.42857142857)*sample_period * float64(-d.gyro_reading.z)) + d.heading)
+	d.heading = float64((sample_period * h1_deg) + d.heading)
+	if d.heading > 360.0 {
+		d.heading = d.heading - float64(360.0)
+	} else if d.heading < 0.0 {
+		d.heading = d.heading + float64(360.0)
+	}
 }
 
-func (d *MPU6050) measure() error {
+func (d *MPU6050) measureGyro() error {
 	XYZ_gyro, err := d.readGyro()
 	if err != nil {
 		return err
 	}
+	d.gyro_reading = XYZ_gyro
+	return nil
+}
+
+func (d *MPU6050) measureAccel() error {
 	XYZ_accel, err := d.readAccel()
 	if err != nil {
 		return err
 	}
-
-	//	glog.V(1).Infof("mpu6050: scaled gyro: (%f, %f, %f)", XYZ_gyro.x, XYZ_gyro.y, XYZ_gyro.z)
-	//	glog.V(1).Infof("mpu6050: scaled accel: (%f, %f, %f)", XYZ_accel.x, XYZ_accel.y, XYZ_accel.z)
-
 	d.accel_reading = XYZ_accel
-	d.gyro_reading = XYZ_gyro
+	return nil
+}
 
+func (d *MPU6050) measure() error {
+	if err := d.measureGyro(); err != nil {
+		return err
+	}
+	if err := d.measureAccel(); err != nil {
+		return err
+	}
 	return nil
 }
 
@@ -204,6 +233,10 @@ func (d *MPU6050) PitchAndRoll() (float64, float64) {
 	return (d.pitch - d.pitch_resting), (d.roll - d.roll_resting)
 }
 
+func (d *MPU6050) Heading() float64 {
+	return d.heading
+}
+
 func (d *MPU6050) Run() {
 	go func() {
 		d.quit = make(chan struct{})
@@ -212,8 +245,8 @@ func (d *MPU6050) Run() {
 		for {
 			select {
 			case <-timer.C:
-				// read values.
-				d.measure()
+				d.measureGyro()
+				d.measureAccel()
 				d.calculatePitchAndRoll()
 			case <-calibrateTimer.C:
 				d.calibrate()
@@ -226,6 +259,12 @@ func (d *MPU6050) Run() {
 	return
 }
 
+// Set heading from a known value (usually GPS true heading).
+func (d *MPU6050) ResetHeading(heading float64) {
+	log.Printf("reset true heading: %f\n", heading)
+	d.heading = heading
+}
+
 // Close.
 func (d *MPU6050) Close() {
 	if d.quit != nil {