Sensor orientation more reliable.

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
+	_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.ReadOne()
-	time.Sleep(500 * time.Millisecond) // Ensure we have enough values
-	_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.Read()
 	if err != nil {
 		return
 	}

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()
 }

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()