diff --git a/main/sensors.go b/main/sensors.go index 1767ee0a..43cc0da7 100644 --- a/main/sensors.go +++ b/main/sensors.go @@ -375,9 +375,7 @@ func makeSensorRotationMatrix(g [3]float64) (rotmat *[3][3]float64) { // This is used in the orientation process where the user specifies the forward and up directions. func getMinAccelDirection() (i int, err error) { - myIMUReader.Read() // Clear out the averages - time.Sleep(500 * time.Millisecond) // Ensure we have enough values - _, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.Read() + _, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.ReadOne() if err != nil { return } diff --git a/sensors/imu.go b/sensors/imu.go index 6c6508e4..cef3eb58 100644 --- a/sensors/imu.go +++ b/sensors/imu.go @@ -9,6 +9,9 @@ type IMUReader interface { // 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) + // ReadOne returns the most recent time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, + // error reading Gyro/Accel, and error reading Mag. + ReadOne() (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 0ad98746..5a17e95a 100644 --- a/sensors/mpu9250.go +++ b/sensors/mpu9250.go @@ -75,6 +75,30 @@ func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, G return } +// ReadOne returns the most recent time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, +// error reading Gyro/Accel, and error reading Mag. +func (m *MPU9250) ReadOne() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) { + var ( + data *mpu9250.MPUData + ) + data = new(mpu9250.MPUData) + + data = <-m.mpu.C + T = data.T.UnixNano() + G1 = data.G1 + G2 = data.G2 + G3 = data.G3 + A1 = data.A1 + A2 = data.A2 + A3 = data.A3 + M1 = data.M1 + M2 = data.M2 + M3 = data.M3 + GAError = data.GAError + MAGError = data.MagError + return +} + // Close stops reading the MPU. func (m *MPU9250) Close() { m.mpu.CloseMPU()