Merge pull request #167 from cyoung/ahrs

Integrate linux-mpu9150 for AHRS.

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "linux-mpu9150"]
+	path = linux-mpu9150
+	url = git://github.com/cyoung/linux-mpu9150

Makefile

@@ -9,7 +9,8 @@ endif
 all:
 	cd dump978 && make lib
 	sudo cp -f ./libdump978.so /usr/lib/libdump978.so
-	go get -t -d -v ./...
+	cd linux-mpu9150 && make -f Makefile-native-shared
+	go get -t -d -v ./main ./test ./linux-mpu9150/mpu ./godump978 ./mpu6050 ./uatparse
 	go build $(BUILDINFO) main/gen_gdl90.go main/traffic.go main/ry835ai.go main/network.go main/managementinterface.go main/sdr.go main/uibroadcast.go
 
 .PHONY: test

circle.yml

@@ -7,7 +7,7 @@ dependencies:
   pre:
     - sudo apt-get update; sudo apt-get install libusb-1.0-0-dev; cd ~/; git clone https://github.com/steve-m/librtlsdr.git; cd librtlsdr; mkdir build; cd build; cmake ../; make; sudo make install; sudo ldconfig; cd ~/; mkdir gopath; cd ~/; mkdir gopath; wget https://storage.googleapis.com/golang/go1.5.1.src.tar.gz; tar -zxvf go1.5.1.src.tar.gz; cd go/src; export GOROOT_BOOTSTRAP=/usr/local/go; ./make.bash; echo $PATH; echo $GOPATH; go version; env
   override:
-    - make
+    - git submodule update --init && make
 
 test:
   override:

main/ry835ai.go

@@ -192,10 +192,6 @@ func processNMEALine(l string) bool {
 				return false
 			}
 			trueCourse = uint16(tc)
-			//FIXME: Experimental. Set heading to true heading on the MPU6050 reader.
-			if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
-				myMPU6050.ResetHeading(float64(tc))
-			}
 		} else {
 			// No movement.
 			mySituation.TrueCourse = 0
@@ -208,6 +204,16 @@ func processNMEALine(l string) bool {
 			return false
 		}
 
+		//FIXME: Experimental. Set heading to true heading on the MPU6050 reader.
+		if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
+			// Only count this heading if a "sustained" >10kts is obtained. This filters out a lot of heading
+			//  changes while on the ground and "movement" is really only changes in GPS fix as it settles down.
+			//TODO: Some more robust checking above current and last speed.
+			if mySituation.GroundSpeed >= 10 && uint16(groundSpeed) >= 10 {
+				myMPU6050.ResetHeading(float64(trueCourse))
+			}
+		}
+
 		mySituation.TrueCourse = uint16(trueCourse)
 		mySituation.GroundSpeed = uint16(groundSpeed)
 		mySituation.LastGroundTrackTime = time.Now()
@@ -366,7 +372,6 @@ func processNMEALine(l string) bool {
 			}
 		}
 
-
 		if x[2] != "A" { // invalid fix
 			return false
 		}
@@ -458,7 +463,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,51 @@
 package mpu6050
 
 import (
+	"../linux-mpu9150/mpu"
+	"log"
 	"math"
 	"time"
-	//	"github.com/golang/glog"
-	"github.com/kidoman/embd"
-	"log"
 )
 
 //https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf
 const (
-	address = 0x68
-
-	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
+	pollDelay = 98 * time.Millisecond // ~10Hz
 )
 
-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
-	gyro_reading  XYZ // "Integrated".
-	accel_reading XYZ // Directly from sensor.
+	pitch float64
+	roll  float64
 
+	// Calibration variables.
+	calibrated    bool
 	pitch_history []float64
 	roll_history  []float64
-
 	pitch_resting float64
 	roll_resting  float64
 
-	pitch   float64
-	roll    float64
-	heading float64
-	//	gyro		chan XYZ
-	//	accel		chan XYZ
-
-	calibrated bool
+	// For tracking heading (mixing GPS track and the gyro output).
+	heading              float64 // Current heading.
+	gps_track            float64 // Last reading directly from the gyro for comparison with current heading.
+	gps_track_valid      bool
+	heading_when_gps_set float64
 
 	quit chan struct{}
 }
 
 // New returns a handle to a MPU6050 sensor.
-func New(bus embd.I2CBus) *MPU6050 {
-	n := &MPU6050{Bus: bus, Poll: pollDelay}
+func New() *MPU6050 {
+	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.
-
-	d.Bus.WriteByteToReg(address, GYRO_CONFIG, 0) // FS_SEL = 0
-	d.Bus.WriteByteToReg(address, ACCEL_CONFIG, 0) // AFS_SEL = 0
+	mpu.InitMPU()
 
 	d.pitch_history = make([]float64, 0)
 	d.roll_history = make([]float64, 0)
@@ -90,6 +58,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 +80,42 @@ 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)
-	if err != nil {
-		return ret, err
-	}
-	y, err := d.Bus.ReadWordFromReg(address, GYRO_YOUT_H)
-	if err != nil {
-		return ret, err
+func normalizeHeading(h float64) float64 {
+	for h < float64(0.0) {
+		h = h + float64(360.0)
 	}
-	z, err := d.Bus.ReadWordFromReg(address, GYRO_ZOUT_H)
-	if err != nil {
-		return ret, err
+	for h >= float64(360.0) {
+		h = h - float64(360.0)
 	}
-
-	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
+	return h
 }
 
-func (d *MPU6050) readAccel() (XYZ, error) {
-	var ret XYZ
+func (d *MPU6050) getMPUData() {
+	pr, rr, hr, err := mpu.ReadMPU()
 
-	x, err := d.Bus.ReadWordFromReg(address, ACCEL_XOUT_H)
-	if err != nil {
-		return ret, err
-	}
-	y, err := d.Bus.ReadWordFromReg(address, ACCEL_YOUT_H)
-	if err != nil {
-		return ret, err
-	}
-	z, err := d.Bus.ReadWordFromReg(address, ACCEL_ZOUT_H)
-	if err != nil {
-		return ret, err
+	// Convert from radians to degrees.
+	pitch := float64(pr) * (float64(180.0) / math.Pi)
+	roll := float64(rr) * (float64(180.0) / math.Pi)
+	heading := float64(hr) * (float64(180.0) / math.Pi)
+	if heading < float64(0.0) {
+		heading = float64(360.0) + heading
 	}
 
-	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)
+	if err == nil {
+		d.pitch = pitch
+		d.roll = roll
 
-	// 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
+		// Calculate the change in direction from current and previous IMU reading.
+		if d.gps_track_valid {
+			d.heading = normalizeHeading((heading - d.heading_when_gps_set) + d.gps_track)
+		} else {
+			d.heading = heading
+		}
+	} else {
+		//		log.Printf("mpu6050.calculatePitchAndRoll(): mpu.ReadMPU() err: %s\n", err.Error())
 	}
-	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.
@@ -243,20 +128,20 @@ func (d *MPU6050) Heading() float64 {
 }
 
 func (d *MPU6050) Run() {
+	time.Sleep(d.Poll)
 	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.measureAccel()
-				d.calculatePitchAndRoll()
-			case <-calibrateTimer.C:
-				d.calibrate()
-				calibrateTimer.Stop()
+				d.getMPUData()
+				//			case <-calibrateTimer.C:
+				//				d.calibrate()
+				//				calibrateTimer.Stop()
 			case <-d.quit:
+				mpu.CloseMPU()
 				return
 			}
 		}
@@ -267,7 +152,9 @@ func (d *MPU6050) Run() {
 // 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
+	d.gps_track = heading
+	d.gps_track_valid = true
+	d.heading_when_gps_set = d.heading
 }
 
 // Close.

test/sensortest.go

@@ -3,13 +3,10 @@ package main
 import (
 	"../mpu6050"
 	"fmt"
-	"github.com/kidoman/embd"
-	_ "github.com/kidoman/embd/host/all"
 	"net"
 	"time"
 )
 
-var bus embd.I2CBus
 var attSensor *mpu6050.MPU6050
 
 func readMPU6050() (float64, float64) {
@@ -19,8 +16,7 @@ func readMPU6050() (float64, float64) {
 }
 
 func initMPU6050() {
-	bus = embd.NewI2CBus(1)
-	attSensor = mpu6050.New(bus)
+	attSensor = mpu6050.New()
 }
 
 func main() {