From 43e6926ddd07ed9a4c3467d67c05f7a1d28e449d Mon Sep 17 00:00:00 2001 From: Phil Glau Date: Wed, 7 Mar 2018 22:42:26 -0800 Subject: [PATCH 1/4] zero mean software calibration --- mpu6050/mpu6050.py | 48 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/mpu6050/mpu6050.py b/mpu6050/mpu6050.py index 8257f72..a5bfb0d 100644 --- a/mpu6050/mpu6050.py +++ b/mpu6050/mpu6050.py @@ -58,6 +58,10 @@ def __init__(self, address, bus=1): self.bus = smbus.SMBus(bus) # Wake up the MPU-6050 since it starts in sleep mode self.bus.write_byte_data(self.address, self.PWR_MGMT_1, 0x00) + # Software Calibration to zero-mean. + # must run self.zero_mean_calibration() on level ground to properly calibrate. + self.use_calibrated_values = False + self.mean_calibrations = [0,0,0,0,0,0] # I2C communication methods @@ -159,6 +163,12 @@ def get_accel_data(self, g = False): y = y / accel_scale_modifier z = z / accel_scale_modifier + if self.use_calibrated_values: + # zero-mean the data + x -= self.mean_calibrations[0] + y -= self.mean_calibrations[1] + z -= self.mean_calibrations[2] + if g is True: return {'x': x, 'y': y, 'z': z} elif g is False: @@ -231,6 +241,12 @@ def get_gyro_data(self): y = y / gyro_scale_modifier z = z / gyro_scale_modifier + if self.use_calibrated_values: + # zero mean the data. (Gyro scope seems to need calibration more than Accel) + x -= self.mean_calibrations[3] + y -= self.mean_calibrations[4] + z -= self.mean_calibrations[5] + return {'x': x, 'y': y, 'z': z} def get_all_data(self): @@ -241,6 +257,38 @@ def get_all_data(self): return [accel, gyro, temp] + def set_calibrated_flag(self,value=True): + ''' + Set to TRUE to used the calculated zero-mean calibration, FALSE + to use the default values. Is initialized to FALSE + :param value: boolean + ''' + self.use_calibrated_values = value + + def zero_mean_calibration(self): + print ("** Calibrating the IMU **") + input ("** Place on level ground and hit Return to continue **") + # number of samples to collect. 200 == approx 5 seconds worth. + N = 200 + import numpy as np + import time + data = np.zeros(shape=(N,6),dtype=np.float) + for i in range(N): + accel = self.get_accel_data(g=True) + gyro = self.get_gyro_data() + if (i % 25 == 0): + print ('.', end= '', flush=True) + data[i, :] = [accel['x'], accel['y'], accel['z'], gyro['x'], gyro['y'], gyro['z']] + # wait 25ms for next sample + time.sleep(25 / 1000.) + # calculate the mean of each reading. + mv = np.mean(data, axis=0) + # compensate for 1g of gravity on 'z' axis. + mv[2] -= 1 + # save the calibrations + self.mean_calibrations = list(mv) + print ("\n** Finished. use set_calibrated_flag() to used calibrated values") + if __name__ == "__main__": mpu = mpu6050(0x68) print(mpu.get_temp()) From 8a513b71d02440558375bfd2242d2349d6f27bbd Mon Sep 17 00:00:00 2001 From: Phil Glau Date: Thu, 8 Mar 2018 20:26:49 -0800 Subject: [PATCH 2/4] removed numpy --- mpu6050/mpu6050.py | 30 ++++++++++++++++++++++-------- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/mpu6050/mpu6050.py b/mpu6050/mpu6050.py index a5bfb0d..92c1a91 100644 --- a/mpu6050/mpu6050.py +++ b/mpu6050/mpu6050.py @@ -6,6 +6,7 @@ """ import smbus +import time class mpu6050: @@ -62,6 +63,8 @@ def __init__(self, address, bus=1): # must run self.zero_mean_calibration() on level ground to properly calibrate. self.use_calibrated_values = False self.mean_calibrations = [0,0,0,0,0,0] + # if return_gravity == FALSE, then m/s^2 are returned + self.return_gravity = False # I2C communication methods @@ -252,7 +255,7 @@ def get_gyro_data(self): def get_all_data(self): """Reads and returns all the available data.""" temp = self.get_temp() - accel = self.get_accel_data() + accel = self.get_accel_data(g=self.return_gravity) gyro = self.get_gyro_data() return [accel, gyro, temp] @@ -270,23 +273,34 @@ def zero_mean_calibration(self): input ("** Place on level ground and hit Return to continue **") # number of samples to collect. 200 == approx 5 seconds worth. N = 200 - import numpy as np - import time - data = np.zeros(shape=(N,6),dtype=np.float) + # initialize the accumulators to 0 + ax,ay,az,gx,gy,gz = [0]*6 + for i in range(N): + # calibrate based on gravity, not m/s^2 accel = self.get_accel_data(g=True) gyro = self.get_gyro_data() if (i % 25 == 0): print ('.', end= '', flush=True) - data[i, :] = [accel['x'], accel['y'], accel['z'], gyro['x'], gyro['y'], gyro['z']] + ax += accel['x'] + ay += accel['y'] + az += accel['z'] + gx += gyro['x'] + gy += gyro['y'] + gz += gyro['z'] # wait 25ms for next sample time.sleep(25 / 1000.) # calculate the mean of each reading. - mv = np.mean(data, axis=0) + ax /= float(N) + ay /= float(N) + az /= float(N) + gx /= float(N) + gy /= float(N) + gz /= float(N) # compensate for 1g of gravity on 'z' axis. - mv[2] -= 1 + az -= 1 # save the calibrations - self.mean_calibrations = list(mv) + self.mean_calibrations = [ax,ay,az,gx,gy,gz] print ("\n** Finished. use set_calibrated_flag() to used calibrated values") if __name__ == "__main__": From 2d6ecbbf87e07ecf50064b4668a260f4806ca2d3 Mon Sep 17 00:00:00 2001 From: Will Roscoe Date: Sun, 8 Jul 2018 21:12:26 +0000 Subject: [PATCH 3/4] cleaned up display of calibration. no longer waits for input --- mpu6050/mpu6050.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mpu6050/mpu6050.py b/mpu6050/mpu6050.py index 92c1a91..551a0d5 100644 --- a/mpu6050/mpu6050.py +++ b/mpu6050/mpu6050.py @@ -270,7 +270,7 @@ def set_calibrated_flag(self,value=True): def zero_mean_calibration(self): print ("** Calibrating the IMU **") - input ("** Place on level ground and hit Return to continue **") + print ("** Place on level ground. re-run is not level at start **") # number of samples to collect. 200 == approx 5 seconds worth. N = 200 # initialize the accumulators to 0 @@ -288,8 +288,8 @@ def zero_mean_calibration(self): gx += gyro['x'] gy += gyro['y'] gz += gyro['z'] - # wait 25ms for next sample - time.sleep(25 / 1000.) + # wait 10ms for next sample + time.sleep(10 / 1000.) # calculate the mean of each reading. ax /= float(N) ay /= float(N) @@ -301,7 +301,9 @@ def zero_mean_calibration(self): az -= 1 # save the calibrations self.mean_calibrations = [ax,ay,az,gx,gy,gz] - print ("\n** Finished. use set_calibrated_flag() to used calibrated values") + print ("\n** Calibration Complete**") + print ('** offsets: ',end='') + print(''.join('{:02.4f} '.format(n) for n in self.mean_calibrations)) if __name__ == "__main__": mpu = mpu6050(0x68) From 246ebf35c4d24add1c5e239b8374d524ae8d6716 Mon Sep 17 00:00:00 2001 From: Will Roscoe Date: Sun, 8 Jul 2018 21:12:26 +0000 Subject: [PATCH 4/4] cleaned up display of calibration. no longer waits for input --- mpu6050/mpu6050.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mpu6050/mpu6050.py b/mpu6050/mpu6050.py index 92c1a91..2be8b89 100644 --- a/mpu6050/mpu6050.py +++ b/mpu6050/mpu6050.py @@ -270,7 +270,7 @@ def set_calibrated_flag(self,value=True): def zero_mean_calibration(self): print ("** Calibrating the IMU **") - input ("** Place on level ground and hit Return to continue **") + print ("** Place on level ground. re-run is not level at start **") # number of samples to collect. 200 == approx 5 seconds worth. N = 200 # initialize the accumulators to 0 @@ -288,8 +288,8 @@ def zero_mean_calibration(self): gx += gyro['x'] gy += gyro['y'] gz += gyro['z'] - # wait 25ms for next sample - time.sleep(25 / 1000.) + # wait 10ms for next sample + time.sleep(10 / 1000.) # calculate the mean of each reading. ax /= float(N) ay /= float(N) @@ -301,7 +301,9 @@ def zero_mean_calibration(self): az -= 1 # save the calibrations self.mean_calibrations = [ax,ay,az,gx,gy,gz] - print ("\n** Finished. use set_calibrated_flag() to used calibrated values") + print ("\n** Calibration Complete **") + print ('** offsets: ',end='') + print(''.join('{:02.4f} '.format(n) for n in self.mean_calibrations)) if __name__ == "__main__": mpu = mpu6050(0x68)