Skip to content

Commit

Permalink
AHRS auto-level working.
Browse files Browse the repository at this point in the history
  • Loading branch information
@westphae
westphae committed Apr 1, 2017
1 parent 1961a3c commit a2580d4
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 58 deletions.
79 changes: 44 additions & 35 deletions main/sensors.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
)

Expand Down Expand Up @@ -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
}

Expand All @@ -149,44 +141,52 @@ 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
for globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected {
<-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:
}
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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.
Expand Down
7 changes: 3 additions & 4 deletions sensors/imu.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
21 changes: 2 additions & 19 deletions sensors/mpu9250.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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()
Expand Down

0 comments on commit a2580d4

Please sign in to comment.