Calibrate gyro

DocGarbanzo · Sep 27, 2024 · b3df03d · b3df03d
1 parent eccc7f1
commit b3df03d
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/donkeycar/parts/imu.py b/donkeycar/parts/imu.py
@@ -146,18 +146,17 @@ def calibrate(self):
         #     time.sleep(0.01)
         # logger.info("Calibration done.")
         # stdout.flush()
-
+        logger.info('Calibrating Mpu6050 ...')
         num_loops = 100
         accel = np.zeros(3)
         gyro = np.zeros(3)
         for _ in range(num_loops):
-            accel += self.mpu.acceleration
             gyro += self.mpu.gyro
             # wait for 25ms
-            time.sleep(0.005)
-        self.accel_zero = accel / num_loops
+            time.sleep(0.01)
+        #self.accel_zero = accel / num_loops
         self.gyro_zero = gyro / num_loops
-        logger.info('... Mpu6050 calibrated')
+        logger.info('Mpu6050 calibrated')
 
     def update(self):
         while self.on:
@@ -186,8 +185,9 @@ def poll(self):
         # if self.ahrs.flags.initialising:
         #     return
         # only record and calculate if not initialising
+        gyro = np.array(self.mpu.gyro) - self.gyro_zero
         self.path.append((new_time,
-                          *self.mpu.gyro,
+                          *gyro,
                           *self.mpu.acceleration))
 
     def run(self):