Use linux-mpu9150 DMP functions to retrieve position data. Remove old POC code.

main/ry835ai.go

@@ -366,7 +366,6 @@ func processNMEALine(l string) bool {
 			}
 		}
 
-
 		if x[2] != "A" { // invalid fix
 			return false
 		}
@@ -458,7 +457,7 @@ func initBMP180() error {
 }
 
 func initMPU6050() error {
-	myMPU6050 = mpu6050.New(i2cbus) //TODO: error checking.
+	myMPU6050 = mpu6050.New() //TODO: error checking.
 	return nil
 }
 

mpu6050/mpu6050.go

@@ -3,83 +3,49 @@
 package mpu6050
 
 import (
-	"math"
+	"../linux-mpu9150/mpu"
-	"time"
-	//	"github.com/golang/glog"
-	"github.com/kidoman/embd"
 	"log"
+	"time"
 )
 
 //https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf
 const (
-	address = 0x68
+	pollDelay = 98 * time.Millisecond // ~10Hz
-
-	GYRO_XOUT_H = 0x43
-	GYRO_YOUT_H = 0x45
-	GYRO_ZOUT_H = 0x47
-
-	ACCEL_XOUT_H = 0x3B
-	ACCEL_YOUT_H = 0x3D
-	ACCEL_ZOUT_H = 0x3F
-
-	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.
-
-	pollDelay = 500 * time.Microsecond // 2000Hz
 )
 
-type XYZ struct {
-	x float32
-	y float32
-	z float32
-}
-
 // MPU6050 represents a InvenSense MPU6050 sensor.
 type MPU6050 struct {
-	Bus  embd.I2CBus
 	Poll time.Duration
 
 	started bool
 
-	//TODO
+	pitch float64
-	gyro_reading  XYZ // "Integrated".
+	roll  float64
-	accel_reading XYZ // Directly from sensor.
 
+	// Calibration variables.
+	calibrated    bool
 	pitch_history []float64
 	roll_history  []float64
-
 	pitch_resting float64
 	roll_resting  float64
 
-	pitch   float64
+	// For tracking heading (mixing GPS track and the gyro output).
-	roll    float64
+	heading                 float64 // Current heading.
-	heading float64
+	last_gyro_heading       float64 // Last reading directly from the gyro for comparison with current heading.
-	//	gyro		chan XYZ
+	last_gyro_heading_valid bool
-	//	accel		chan XYZ
-
-	calibrated bool
 
 	quit chan struct{}
 }
 
 // New returns a handle to a MPU6050 sensor.
-func New(bus embd.I2CBus) *MPU6050 {
+func New() *MPU6050 {
-	n := &MPU6050{Bus: bus, Poll: pollDelay}
+	n := &MPU6050{Poll: pollDelay}
 	n.StartUp()
 	return n
 }
 
-//TODO
 func (d *MPU6050) StartUp() error {
-	d.Bus.WriteByteToReg(address, PWR_MGMT_1, 0) // Wake device up.
+	mpu.InitMPU()
-
-	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)
@@ -90,6 +56,8 @@ func (d *MPU6050) StartUp() error {
 	return nil
 }
 
+/*
+
 func (d *MPU6050) calibrate() {
 	//TODO: Error checking to make sure that the histories are extensive enough to be significant.
 	//TODO: Error checking to do continuous calibrations.
@@ -110,127 +78,37 @@ func (d *MPU6050) calibrate() {
 	d.calibrated = true
 }
 
-func (d *MPU6050) readGyro() (XYZ, error) {
+*/
-	var ret XYZ
 
-	x, err := d.Bus.ReadWordFromReg(address, GYRO_XOUT_H)
+func normalizeHeading(h float64) float64 {
-	if err != nil {
+	for h < float64(0.0) {
-		return ret, err
+		h = h + float64(360.0)
 	}
-	y, err := d.Bus.ReadWordFromReg(address, GYRO_YOUT_H)
+	for h >= float64(360.0) {
-	if err != nil {
+		h = h - float64(360.0)
-		return ret, err
 	}
-	z, err := d.Bus.ReadWordFromReg(address, GYRO_ZOUT_H)
+	return h
-	if err != nil {
-		return ret, err
-	}
-
-	ret.x = float32(int16(x)) / GYRO_SCALE // º/sec
-	ret.y = float32(int16(y)) / GYRO_SCALE // º/sec
-	ret.z = float32(int16(z)) / GYRO_SCALE // º/sec
-
-	return ret, nil
 }
 
-func (d *MPU6050) readAccel() (XYZ, error) {
+func (d *MPU6050) getMPUData() {
-	var ret XYZ
+	p, r, h, err := mpu.ReadMPU()
 
-	x, err := d.Bus.ReadWordFromReg(address, ACCEL_XOUT_H)
+	if err == nil {
-	if err != nil {
+		d.pitch = float64(p)
-		return ret, err
+		d.roll = float64(r)
+
+		// Calculate the change in direction from current and previous IMU reading.
+		if d.last_gyro_heading_valid {
+			h2 := float64(h)
+			this_change := h2 - d.last_gyro_heading
+
+			d.heading = normalizeHeading(d.heading + this_change)
 		}
-	y, err := d.Bus.ReadWordFromReg(address, ACCEL_YOUT_H)
+		d.last_gyro_heading_valid = true
-	if err != nil {
+		d.last_gyro_heading = float64(h)
-		return ret, err
+	} else {
+		log.Printf("mpu6050.calculatePitchAndRoll(): mpu.ReadMPU() err: %s\n", err.Error())
 	}
-	z, err := d.Bus.ReadWordFromReg(address, ACCEL_ZOUT_H)
-	if err != nil {
-		return ret, err
-	}
-
-	ret.x = float32(int16(x)) / ACCEL_SCALE
-	ret.y = float32(int16(y)) / ACCEL_SCALE
-	ret.z = float32(int16(z)) / ACCEL_SCALE
-
-	return ret, nil
-}
-
-func (d *MPU6050) calculatePitchAndRoll() {
-	accel := d.accel_reading
-	//	log.Printf("accel: %f, %f, %f\n", accel.x, accel.y, accel.z)
-
-	// Accel.
-
-	p1 := math.Atan2(float64(accel.y), dist(accel.x, accel.z))
-	p1_deg := p1 * (180.0 / math.Pi)
-
-	r1 := math.Atan2(float64(accel.x), dist(accel.y, accel.z))
-	r1_deg := -r1 * (180.0 / math.Pi)
-
-	// Gyro.
-
-	p2 := float64(d.gyro_reading.x)
-	r2 := float64(d.gyro_reading.y) // Backwards?
-
-	// "Noise filter".
-	ft := float64(0.98)
-	sample_period := float64(1 / 2000.0)
-	d.pitch = float64(ft*(sample_period*p2+d.pitch) + (1-ft)*p1_deg)
-	d.roll = float64((ft*(sample_period*r2+d.roll) + (1-ft)*r1_deg))
-
-	if !d.calibrated {
-		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) 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
-	}
-	d.accel_reading = XYZ_accel
-	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
-}
-
-func dist(a, b float32) float64 {
-	a64 := float64(a)
-	b64 := float64(b)
-	return math.Sqrt((a64 * a64) + (b64 * b64))
 }
 
 // Temperature returns the current temperature reading.
@@ -246,16 +124,14 @@ func (d *MPU6050) Run() {
 	go func() {
 		d.quit = make(chan struct{})
 		timer := time.NewTicker(d.Poll)
-		calibrateTimer := time.NewTicker(1 * time.Minute)
+		//		calibrateTimer := time.NewTicker(1 * time.Minute)
 		for {
 			select {
 			case <-timer.C:
-				d.measureGyro()
+				d.getMPUData()
-				d.measureAccel()
+				//			case <-calibrateTimer.C:
-				d.calculatePitchAndRoll()
+				//				d.calibrate()
-			case <-calibrateTimer.C:
+				//				calibrateTimer.Stop()
-				d.calibrate()
-				calibrateTimer.Stop()
 			case <-d.quit:
 				return
 			}