kopia lustrzana https://github.com/cyoung/stratux
Use linux-mpu9150 DMP functions to retrieve position data. Remove old POC code.
Christopher Young
rodzic
9f66732bfb
commit
a35e3cb76f
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
y, err := d.Bus.ReadWordFromReg(address, ACCEL_YOUT_H)
|
||||
if err != nil {
|
||||
return ret, err
|
||||
d.last_gyro_heading_valid = true
|
||||
d.last_gyro_heading = float64(h)
|
||||
} 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.measureAccel()
|
||||
d.calculatePitchAndRoll()
|
||||
case <-calibrateTimer.C:
|
||||
d.calibrate()
|
||||
calibrateTimer.Stop()
|
||||
d.getMPUData()
|
||||
// case <-calibrateTimer.C:
|
||||
// d.calibrate()
|
||||
// calibrateTimer.Stop()
|
||||
case <-d.quit:
|
||||
return
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue