mpu.Calibrate takes a number of retries

main/ry83Xai.go

@@ -1406,6 +1406,8 @@ func initMPU() error {
 			time.Sleep(100 * time.Millisecond)
 		} else {
 			log.Println("AHRS: Successfully initialized MPU9250")
+			time.Sleep(time.Second)
+			myMPU.Calibrate(1, 5)
 			return nil
 		}
 	}

mpu/mpu.go

@@ -11,5 +11,5 @@ type MPU interface {
 	RateOfTurn() (float64, error)
 	GLoad() (float64, error)
 	ReadRaw() (int64, float64, float64, float64, float64, float64, float64, float64, float64, float64, error, error)
-	Calibrate(int) error
+	Calibrate(int, int) error
 }

mpu/mpu6050.go

@@ -205,6 +205,6 @@ func (d *MPU6050) ReadRaw() (int64, float64, float64, float64, float64, float64,
 		errors.New("Error: ReadRaw() not implemented yet for MPU6050"),
 		errors.New("Error: MPU6050 magnetometer isn't working on RY835AI chip")
 }
-func (d *MPU6050) Calibrate(dur int) error {
+func (d *MPU6050) Calibrate(dur int, retries int) error {
     return nil //TODO westphae: for now, maybe we'll get lucky; but eventually we should calibrate
 }

mpu/mpu9250.go

@@ -182,9 +182,15 @@ func (m *MPU9250) ReadRaw() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64
 	return
 }
 
-func (m *MPU9250) Calibrate(dur int) (err error) {
-	m.mpu.CCal<- dur
-	err = <-m.mpu.CCalResult
+func (m *MPU9250) Calibrate(dur, retries int) (err error) {
+	for i:=0; i<retries; i++ {
+		m.mpu.CCal<- dur
+		err = <-m.mpu.CCalResult
+		if err == nil {
+			break
+		}
+		time.Sleep(time.Duration(50) * time.Millisecond)
+	}
 	return
 }