From 4a277b1ee166e404e26e72866d7f09a005bf47f4 Mon Sep 17 00:00:00 2001
From: juliangaal <juliangaal@protonmail.com>
Date: Tue, 23 Apr 2019 18:24:02 +0200
Subject: [PATCH] restructure functions to better reflect typical usage

---
 src/lib.rs | 48 ++++++++++++++++++++++++------------------------
 1 file changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 6c2300f..4e73fcb 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -149,6 +149,29 @@ where
         }
     }
 
+    /// Wakes MPU6050 with all sensors enabled (default)
+    pub fn wake(&mut self) -> Result<(), Mpu6050Error<E>> {
+        self.write_u8(POWER_MGMT_1, 0)?;
+        self.delay.delay_ms(100u8);
+        Ok(())
+    }
+
+    /// Init wakes MPU6050 and verifies register addr, e.g. in i2c
+    pub fn init(&mut self) -> Result<(), Mpu6050Error<E>> {
+        self.wake()?;
+        self.verify()?;
+        Ok(())
+    }
+
+    /// Verifies device to address 0x68 with WHOAMI Register
+    pub fn verify(&mut self) -> Result<(), Mpu6050Error<E>> {
+        let address = self.read_u8(WHOAMI)?;
+        if address != SLAVE_ADDR {
+            return Err(Mpu6050Error::InvalidChipId(address));
+        }
+        Ok(())
+    }
+
     /// Performs software calibration with steps number of readings
     /// of accelerometer and gyrometer sensor
     /// Readings must be made with MPU6050 in resting position
@@ -175,7 +198,7 @@ where
     /// number of readings: accelerometer, gyro and temperature sensor each
     pub fn calc_variance(&mut self, steps: u8) -> Result<(), Mpu6050Error<E>> {
         if let None = self.bias {
-            self.soft_calib(100)?;
+            self.soft_calib(steps)?;
         }
         
         let mut variance = Variance::default();
@@ -209,29 +232,6 @@ where
         self.variance.as_ref()
     }
 
-    /// Wakes MPU6050 with all sensors enabled (default)
-    pub fn wake(&mut self) -> Result<(), Mpu6050Error<E>> {
-        self.write_u8(POWER_MGMT_1, 0)?;
-        self.delay.delay_ms(100u8);
-        Ok(())
-    }
-
-    /// Init wakes MPU6050 and verifies register addr, e.g. in i2c
-    pub fn init(&mut self) -> Result<(), Mpu6050Error<E>> {
-        self.wake()?;
-        self.verify()?;
-        Ok(())
-    }
-
-    /// Verifies device to address 0x68 with WHOAMI Register
-    pub fn verify(&mut self) -> Result<(), Mpu6050Error<E>> {
-        let address = self.read_u8(WHOAMI)?;
-        if address != SLAVE_ADDR {
-            return Err(Mpu6050Error::InvalidChipId(address));
-        }
-        Ok(())
-    }
-
     /// Roll and pitch estimation from raw accelerometer readings
     /// NOTE: no yaw! no magnetometer present on MPU6050
     pub fn get_acc_angles(&mut self) -> Result<(f32, f32), Mpu6050Error<E>> {