From a35e3cb76fba6649c7c3efbf0c10f6903b94a09c Mon Sep 17 00:00:00 2001 From: Christopher Young Date: Sun, 27 Dec 2015 16:11:21 -0500 Subject: [PATCH] Use linux-mpu9150 DMP functions to retrieve position data. Remove old POC code. --- main/ry835ai.go | 3 +- mpu6050/mpu6050.go | 214 ++++++++++----------------------------------- 2 files changed, 46 insertions(+), 171 deletions(-) diff --git a/main/ry835ai.go b/main/ry835ai.go index d12aa140..d505e1f3 100644 --- a/main/ry835ai.go +++ b/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 } diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go index 3d3e6152..280aa310 100644 --- a/mpu6050/mpu6050.go +++ b/mpu6050/mpu6050.go @@ -3,83 +3,49 @@ package mpu6050 import ( - "math" - "time" - // "github.com/golang/glog" - "github.com/kidoman/embd" + "../linux-mpu9150/mpu" "log" + "time" ) //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. + last_gyro_heading float64 // Last reading directly from the gyro for comparison with current heading. + last_gyro_heading_valid bool 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 +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) - if err != nil { - return ret, err +func normalizeHeading(h float64) float64 { + for h < float64(0.0) { + h = h + float64(360.0) } - y, err := d.Bus.ReadWordFromReg(address, GYRO_YOUT_H) - if err != nil { - return ret, err + for h >= float64(360.0) { + h = h - float64(360.0) } - z, err := d.Bus.ReadWordFromReg(address, GYRO_ZOUT_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 + return h } -func (d *MPU6050) readAccel() (XYZ, error) { - var ret XYZ +func (d *MPU6050) getMPUData() { + p, r, h, err := mpu.ReadMPU() - x, err := d.Bus.ReadWordFromReg(address, ACCEL_XOUT_H) - if err != nil { - return ret, err + if err == nil { + d.pitch = float64(p) + 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) + } + d.last_gyro_heading_valid = true + d.last_gyro_heading = float64(h) + } else { + log.Printf("mpu6050.calculatePitchAndRoll(): mpu.ReadMPU() err: %s\n", err.Error()) } - 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 - } - - 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.measureAccel() - d.calculatePitchAndRoll() - case <-calibrateTimer.C: - d.calibrate() - calibrateTimer.Stop() + d.getMPUData() + // case <-calibrateTimer.C: + // d.calibrate() + // calibrateTimer.Stop() case <-d.quit: return }