From a2580d4593931d35cbac8e1b474a4ad52feb5c70 Mon Sep 17 00:00:00 2001 From: Eric Westphal Date: Sat, 1 Apr 2017 14:02:12 -0400 Subject: [PATCH] AHRS auto-level working. --- main/sensors.go | 79 ++++++++++++++++++++++++++-------------------- sensors/imu.go | 7 ++-- sensors/mpu9250.go | 21 ++---------- 3 files changed, 49 insertions(+), 58 deletions(-) diff --git a/main/sensors.go b/main/sensors.go index d0b7a5be1..d15d3a982 100644 --- a/main/sensors.go +++ b/main/sensors.go @@ -72,10 +72,10 @@ func tempAndPressureSender() { var ( temp float64 press float64 - altLast float64 = -9999 + altLast = -9999.9 altitude float64 err error - dt float64 = 0.1 + dt = 0.1 failnum uint8 ) @@ -125,21 +125,13 @@ func initIMU() (ok bool) { if err == nil { myIMUReader = imu time.Sleep(200 * time.Millisecond) - log.Println("AHRS Info: Successfully connected MPU9250, running calibration") - ahrsCalibrating = true - if err := myIMUReader.Calibrate(1, 1); err == nil { - log.Println("AHRS Info: Successfully calibrated MPU9250") - ahrsCalibrating = false - return true - } - log.Println("AHRS Info: couldn't calibrate MPU9250") - ahrsCalibrating = false - return false + log.Println("AHRS Info: Successfully connected MPU9250") + return true } - // TODO westphae: try to connect to MPU9150 + // TODO westphae: try to connect to MPU9150 or other IMUs. - log.Println("AHRS Error: couldn't initialize MPU9250 or MPU9150") + log.Println("AHRS Error: couldn't initialize MPU9250") return false } @@ -149,28 +141,34 @@ func sensorAttitudeSender() { t time.Time s ahrs.AHRSProvider m *ahrs.Measurement - a1, a2, a3, b1, b2, b3, m1, m2, m3 float64 // IMU measurements - ff *[3][3]float64 // Sensor orientation matrix + a1, a2, a3, b1, b2, b3, m1, m2, m3 float64 // IMU measurements + c, d [3]float64 // IMU calibration measurements + ff [3][3]float64 // Sensor orientation matrix + cc float64 mpuError, magError error failnum uint8 ) - log.Println("AHRS Info: initializing new simple AHRS") + log.Println("AHRS Info: initializing new Simple AHRS") s = ahrs.InitializeSimple() m = ahrs.NewMeasurement() - cage = make(chan (bool)) + cage = make(chan (bool), 1) // Set up loggers for analysis ahrswebListener, err := ahrsweb.NewKalmanListener() if err != nil { - log.Printf("AHRS Error: couldn't start ahrswebListener: %s\n", err.Error()) + log.Printf("AHRS Info: couldn't start ahrswebListener: %s\n", err.Error()) } else { + log.Println("AHRS Info: ahrswebListener started on port 8000") defer ahrswebListener.Close() } // Need a sampling freq faster than 10Hz timer := time.NewTicker(50 * time.Millisecond) // ~20Hz update. for { - ff = makeSensorRotationMatrix(m) + select { // Don't block if cage isn't receiving: only need one cage in the queue at a time. + case cage <- true: + default: + } failnum = 0 <-timer.C @@ -178,15 +176,17 @@ func sensorAttitudeSender() { <-timer.C select { case <-cage: + log.Println("AHRS Info: Calibrating IMU") ahrsCalibrating = true - if err := myIMUReader.Calibrate(1, 1); err == nil { - log.Println("AHRS Info: Successfully recalibrated MPU9250") - ff = makeSensorRotationMatrix(m) - - } else { - log.Println("AHRS Info: couldn't recalibrate MPU9250") - } + //TODO westphae: check for errors when reading IMU + myIMUReader.Read() // Clear out the averages + time.Sleep(1 * time.Second) + _, d[0], d[1], d[2], c[0], c[1], c[2], _, _, _, _, _ = myIMUReader.Read() + ff = *makeSensorRotationMatrix([3]float64{c[0], c[1], c[2]}) + log.Printf("AHRS Info: IMU Calibrated: accel %6f %6f %6f; gyro %6f %6f %6f\n", + c[0], c[1], c[2], d[0], d[1], d[2]) ahrsCalibrating = false + cc = math.Sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]) s.Reset() default: } @@ -195,6 +195,12 @@ func sensorAttitudeSender() { m.T = float64(t.UnixNano()/1000) / 1e6 _, b1, b2, b3, a1, a2, a3, m1, m2, m3, mpuError, magError = myIMUReader.Read() + a1 /= cc + a2 /= cc + a3 /= cc + b1 -= d[0] + b2 -= d[1] + b3 -= d[2] m.A1 = -(ff[0][0]*a1 + ff[0][1]*a2 + ff[0][2]*a3) m.A2 = -(ff[1][0]*a1 + ff[1][1]*a2 + ff[1][2]*a3) m.A3 = -(ff[2][0]*a1 + ff[2][1]*a2 + ff[2][2]*a3) @@ -263,7 +269,7 @@ func sensorAttitudeSender() { s.Reset() } - // Debugging server: + // Send to AHRS debugging server: if ahrswebListener != nil { if err = ahrswebListener.Send(s.GetState(), m); err != nil { log.Printf("Error writing to ahrsweb: %s\n", err) @@ -289,7 +295,7 @@ func sensorAttitudeSender() { } } -func makeSensorRotationMatrix(mCal *ahrs.Measurement) *[3][3]float64 { +func makeSensorRotationMatrix(g [3]float64) *[3][3]float64 { f := globalSettings.IMUMapping if globalSettings.IMUMapping[0] == 0 { // if unset, default to some standard orientation globalSettings.IMUMapping[0] = -1 // +2 for RY836AI @@ -312,26 +318,29 @@ func makeSensorRotationMatrix(mCal *ahrs.Measurement) *[3][3]float64 { } else { z[+f[1]-1] = +1 } - //TODO westphae: replace "up direction" with measured gravity. + if math.Abs(z[0]*g[0]+z[1]*g[1]+z[2]*g[2]-1) > 0.1 { + log.Println("AHRS Warning: sensor is not nearly level with chosen up direction") + } + z = g // Normalize the gravity vector to be 1 G. - gg := z[0]*z[0] + z[1]*z[1] + z[2]*z[2] + gg := math.Sqrt(z[0]*z[0] + z[1]*z[1] + z[2]*z[2]) z[0] /= gg z[1] /= gg z[2] /= gg // Remove the projection on the measured gravity vector from x so it's orthogonal to z. dp := x[0]*z[0] + x[1]*z[1] + x[2]*z[2] - x[0] = x[0] - dp * z[0] - x[1] = x[1] - dp * z[1] - x[2] = x[2] - dp * z[2] + x[0] = x[0] - dp*z[0] + x[1] = x[1] - dp*z[1] + x[2] = x[2] - dp*z[2] // Specify the "left wing" direction for a right-handed coordinate system using the cross product. y[0] = z[1]*x[2] - z[2]*x[1] y[1] = z[2]*x[0] - z[0]*x[2] y[2] = z[0]*x[1] - z[1]*x[0] - return &[3][3]float64 {x, y, z} + return &[3][3]float64{x, y, z} } // This is used in the orientation process where the user specifies the forward and up directions. diff --git a/sensors/imu.go b/sensors/imu.go index 86f755e18..6c6508e45 100644 --- a/sensors/imu.go +++ b/sensors/imu.go @@ -3,12 +3,11 @@ package sensors // IMUReader provides an interface to various Inertial Measurement Unit sensors, // such as the InvenSense MPU9150 or MPU9250. It is a light abstraction on top -// of the current goflying MPU9250 driver so that it can accommodate other types +// of the current github.com/westphae/goflying MPU9250 driver so that it can accommodate other types // of drivers. type IMUReader interface { - // Calibrate kicks off a calibration for specified duration (s) and retries. - Calibrate(duration, retries int) error - // Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, error reading Gyro/Accel, and error reading Mag. + // Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, + // error reading Gyro/Accel, and error reading Mag. Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MagError error) // Close stops reading the MPU. Close() diff --git a/sensors/mpu9250.go b/sensors/mpu9250.go index 08f6464d3..a35c0a026 100644 --- a/sensors/mpu9250.go +++ b/sensors/mpu9250.go @@ -47,7 +47,8 @@ func NewMPU9250() (*MPU9250, error) { return &m, nil } -// Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, error reading Gyro/Accel, and error reading Mag. +// Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, +// error reading Gyro/Accel, and error reading Mag. func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) { data := <-m.mpu.CAvg T = data.T.UnixNano() @@ -65,24 +66,6 @@ func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, G return } -// Calibrate kicks off a calibration for specified duration (s) and retries. -func (m *MPU9250) Calibrate(dur, retries int) (err error) { - if dur > 0 { - for i := 0; i < retries; i++ { - m.mpu.CCal <- dur - log.Printf("AHRS Info: Waiting for calibration result try %d of %d\n", i+1, retries) - err = <-m.mpu.CCalResult - if err == nil { - log.Println("AHRS Info: MPU9250 calibrated") - break - } - time.Sleep(time.Duration(50) * time.Millisecond) - log.Println("AHRS Info: MPU9250 wasn't inertial, retrying calibration") - } - } - return -} - // Close stops reading the MPU. func (m *MPU9250) Close() { m.mpu.CloseMPU()