Measuring Level
Its been a while since I had time to work on this project. Its been a busy summer.
I got the accelerameter installed and code written to calibrate the orientation of the sensor to that of the box it is in. I can measure pitch and roll within 0.05 degrees repeatably. I can use this to calculate how many blocks i nees to stack and at which wheels. Later i can use this to jack my rv with leveling legs.
I used an mpu6050 which is about $1 on ebay and talks over I2C.
Heres the start of the library i wrote to calibrate and measure pitch and roll:
from mpu6050 import mpu6050
import numpy as np
from scipy.optimize import minimize
from scipy.linalg import expm3, norm
import os
sensor = mpu6050(0x68)
class AccellGyro(mpu6050):
def __init__(self):
mpu6050.__init__(self, 0x68)
self.calFile = 'accelCal.txt'
self.angCorFile = 'angCor.txt'
if not os.path.exists('./' + self.calFile) or not os.path.exists('./'+self.angCorFile):
self.angCor = np.asarray([0,0])
self.setCalibration()
else:
self.rotationMatrix = np.loadtxt('./'+self.calFile)
self.angCor = np.loadtxt('./'+self.angCorFile)
def measureTilt(self, nSamples=1000, vect=None):
if vect is None:
vect = self._measureRaw(nSamples)
raw = self._measureRaw(nSamples)
print(raw)
meas = np.dot(self.rotationMatrix, vect)
print(meas)
roll = np.arctan(-meas[0]/meas[2])
pitch = np.arctan(meas[1]/np.sqrt(np.power(meas[0],2)+np.power(meas[2],2)))
return np.asarray([pitch, roll])+self.angCor
def _measureRaw(self, nSamples=1000, norm=True):
data = np.zeros((nSamples, 3))
for i in range(nSamples):
x = self.get_accel_data()
data[i,:] = np.asarray([x['x'], x['y'], x['z']])
if norm:
meas = np.mean(data, 0)
meas /= np.sqrt(np.sum(np.power(meas, 2)))
return meas
def setCalibration(self):
print('Calibration file not found. Starting Calibration Proceedure.')
while True:
r = raw_input('On a flat, level surface, place sensor box upright, y to continue: ')
if r == 'y':
break
a = self._measureRaw(5000)
while True:
r = raw_input('On a flat, level surface, place sensor box on the right face, y to continue: ')
if r == 'y':
break
b = self._measureRaw(5000)
while True:
r = raw_input('On a flat, level surface, place sensor box on the front face, y to continue: ')
if r == 'y':
break
c = self._measureRaw(5000)
calInList = [a, b, c]
calInListOrder = [np.where(a == a.max())[0][0], np.where(b == b.max())[0][0], np.where(c == c.max())[0][0]]
calInList = np.asarray([list(calInList[calInListOrder.index(i)]) for i in range(3)])
self.mask = 1.0 - np.identity(3)
print
print('Measurements complete, starting optimization....')
ret = minimize(self.calibObjective, [0,0,0], args=(calInList[0,:],calInList[1,:],calInList[2,:]), tol=1e-10)
print(ret.message)
print
print('Optimized Calibration Angles (deg):')
ang = ret.x
print(ang*180.0/(np.pi))
print
self.rotationMatrix = self.rotate3D(*ang)
print('Calibration complete! Final Rotation Matrix:')
print(self.rotationMatrix)
print
self.angCor = -1.0*self.measureTilt(vect=a)
print('Angle Correction (deg):')
print(self.angCor*180.0/np.pi)
print
np.savetxt('./'+self.calFile, self.rotationMatrix)
np.savetxt('./'+self.angCorFile, self.angCor)
def rotateAboutAxis(self, axis, theta):
return expm3(np.cross(np.eye(3), axis/norm(axis)*theta))
def rotate3D(self, a, b, c):
return np.dot(np.dot(self.rotateAboutAxis([1,0,0], a), self.rotateAboutAxis([0,1,0], b)), self.rotateAboutAxis([0,0,1], c))
def calibObjective(self, x, *args):
args = np.asarray(args)
#print('args')
#print(args)
rm = self.rotate3D(*x)
rot = np.dot(rm, args)
#print(rot)
mult = np.multiply(self.mask, rot)
err1 = np.sum(np.abs(mult))
return -1.0*(rot[0,0]+rot[1,1]+rot[2,2]) + err1
if __name__ == '__main__':
s = AccellGyro()
print('roll and pitch:')
print(s.measureTilt()*180.0/np.pi)