More robust acceleration detection for orientation.
@@ -50,7 +50,14 @@ func NewMPU9250() (*MPU9250, 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.
func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
- data := <-m.mpu.CAvg
+ var (
+ data *mpu9250.MPUData
+ i int8
+ )
+ data = new(mpu9250.MPUData)
+
+ for data.N==0 && i < 5 {
+ data = <-m.mpu.CAvg
T = data.T.UnixNano()
G1 = data.G1
G2 = data.G2
@@ -63,6 +70,8 @@ func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, G
M3 = data.M3
GAError = data.GAError
MAGError = data.MagError
+ i++
+ }
return
}
