def test_verbose_prints_progress(points_2, capsys):
c = Calibraxis(verbose=True)
c.add_points(points_2)
c.calibrate_accelerometer()
out, err = capsys.readouterr()
for row in filter(None, out.split('\n')):
assert re.match('^([0-9]+):\s([0-9\-\.e]+)\s*(\([0-9\s\-\.e,]+\))$', row)
def test_add_points_4(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2 ** 15) / 8.)
c.add_points(points.tolist())
np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_add_points_3(points_1):
c = Calibraxis(verbose=False)
points = points_1 / ((2 ** 15) / 8.)
for p in points:
c.add_points(tuple(p))
np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_recalibration_points_2(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2**15) / 16.)
for p in points[:-1, :]:
c.add_points(p)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
c.add_points(points[-1, :])
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_recalibration_points_2(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2 ** 15) / 16.)
for p in points[:-1, :]:
c.add_points(p)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
c.add_points(points[-1, :])
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_add_points_5(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2**15) / 8.)
c.add_points(points)
c.add_points([])
np.testing.assert_almost_equal(
np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_batch_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
out = c.batch_apply(points_1)
normed = np.sqrt((np.array(out)**2).sum(axis=1))
np.testing.assert_array_almost_equal(normed, 1.0, 2)
def start_calculation(self):
t = time.time()
c = Calibraxis()
points = []
for _ in range(0):
print("calibrating", points)
ax, ay, az = self.mpu.get_accel_data(True)
points.append([ax, ay, az])
# print (ax,ay,az)
ax1 = ax + g * math.sin(self.statespace.roll)
ay1 = ay + g * math.sin(self.statespace.pitch)
az1 = az + g * (math.cos(self.statespace.roll) *
math.cos(self.statespace.pitch))
# print (ax1,ay1,az1)
# print (self.statespace.roll,self.statespace.pitch,self.statespace.yaw)
t1 = time.time()
self.statespace.vx = ax1
self.statespace.vy = ay1
self.statespace.vz = az1
t = t1
yield from asyncio.sleep(dt)
print(self.statespace.vx, self.statespace.vy, self.statespace.vz)
now = time.time()
# print("error in time", dt - (t-now))
# print(points)
# c.add_points(points)
#c .calibrate_accelerometer()
while True:
ax, ay, az = self.mpu.get_accel_data()
# print ("acce",ax,ay,az)
# ax,ay,az=c.apply([ax,ay,az])
# print ("aftercalibrate",ax,ay,az)
print("roll: ", self.statespace.roll, "pitch",
self.statespace.pitch)
print("acc", ax, ay, az)
ax1 = ax - g * math.sin(math.radians(self.statespace.pitch))
ay1 = ay - g * math.sin(math.radians(self.statespace.roll))
az1 = az + g * (math.cos(math.radians(self.statespace.roll)) *
math.cos(math.radians(self.statespace.pitch)))
print(ax1, ay1, az1)
print("")
# print (self.statespace.roll,self.statespace.pitch,self.statespace.yaw)
t1 = time.time()
self.statespace.vx = ax1
self.statespace.vy = ay1
self.statespace.vz = az1
t = t1
yield from asyncio.sleep(3)
# print (self.statespace.vx,self.statespace.vy,self.statespace.vz)
now = time.time()
def test_batch_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
out = c.batch_apply(points_1)
normed = np.sqrt((np.array(out) ** 2).sum(axis=1))
np.testing.assert_array_almost_equal(normed, 1.0, 2)
def test_verbose_prints_progress(points_2, capsys):
c = Calibraxis(verbose=True)
c.add_points(points_2)
c.calibrate_accelerometer()
out, err = capsys.readouterr()
for row in filter(None, out.split('\n')):
assert re.match('^([0-9]+):\s([0-9\-\.e]+)\s*(\([0-9\s\-\.e,]+\))$',
row)
def test_add_points_3(points_1):
c = Calibraxis(verbose=False)
points = points_1 / ((2**15) / 8.)
for p in points:
c.add_points(tuple(p))
np.testing.assert_almost_equal(
np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
from calibraxis import Calibraxis
import time
import math
import mpu6050
import numpy as np
# Sensor initialization
mpu = mpu6050.MPU6050()
mpu.dmpInitialize()
mpu.setDMPEnabled(True)
# get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize()
array = np.array()
a = Calibraxis()
g = Calibraxis()
apoints = np.array([[0, 0, 0]])
gpoints = np.array([[0, 0, 0]])
for _ in range(1,100):
# Get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus()
if mpuIntStatus >= 2: # check for DMP data ready interrupt (this should happen frequently)
# get current FIFO count
fifoCount = mpu.getFIFOCount()
# check for overflow (this should never happen unless our code is too inefficient)
if fifoCount == 1024:
import numpy as np
from calibraxis import Calibraxis
c = Calibraxis()
points = np.array([[-4772.38754098, 154.04459016, -204.39081967],
[3525.0346179, -68.64924886, -34.54604833],
[-658.17681729, -4137.60248854, -140.49377865],
[-564.18562092, 4200.29150327, -130.51895425],
[-543.18289474, 18.14736842, -4184.43026316],
[-696.62532808, 15.70209974, 3910.20734908],
[406.65271419, 18.46827992, -4064.61085677],
[559.45926413, -3989.69513798, -174.71879106],
[597.22629169, -3655.54153041, -1662.83257031],
[1519.02616089, -603.82472204, 3290.58469588]])
# Add points to calibration object's storage.
c.add_points(points)
# Run the calibration parameter optimization.
c.calibrate_accelerometer()
# Applying the calibration parameters to the calibration data.
c.apply(points[0:])
c.batch_apply(points)
def test_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(np.linalg.norm(c.apply(points_1[0, :])),
1.0, 2)
def test_calibration_points_2_scaled(points_2):
c = Calibraxis(verbose=False)
c.add_points(points_2 / ((2 ** 15) / 16.))
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_calibration_points_1(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_error_to_few_points(points_2):
c = Calibraxis(verbose=False)
for p in points_2[:5, :]:
c.add_points(p)
with pytest.raises(ValueError):
c.calibrate_accelerometer()
def test_error_to_few_points(points_2):
c = Calibraxis(verbose=False)
for p in points_2[:5, :]:
c.add_points(p)
with pytest.raises(ValueError):
c.calibrate_accelerometer()
def test_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(np.linalg.norm(c.apply(points_1[0, :])), 1.0, 2)
"""
import numpy as np
from numpy import genfromtxt
#import matplotlib
#matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import scipy as scipy
from scipy import optimize
import warnings
from numpy.linalg import norm
from quaternion import Quaternion
from calibraxis import Calibraxis
#matplotlib.use('TkAgg')
c = Calibraxis()
cal_points = np.array([[0.07778838, 0.02505269, 0.88501839],
[0.73871366, 0.0255498, -0.81387024],
[-0.95695496, -0.01158575, 0.11527143],
[0.42802767, 0.75920515, -0.6072115],
[0.99437399, -0.11401009, 0.02588071],
[-0.04117689, 0.20844233, 0.85707983],
[0.85112945, 0.02989109, -0.67080324],
[0.90229526, 0.25129969, -0.49528233],
[-0.25559073, -0.28405265, 0.80770797],
[0.99941998, 0.03766609, -0.04686234],
[0.04151319, 1.0228318, 0.14966903]])
# Add points to calibration object's storage.
c.add_points(cal_points)
# Run the calibration parameter optimization.
c.calibrate_accelerometer()
def test_calibration_points_1(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_calibration_points_2_scaled(points_2):
c = Calibraxis(verbose=False)
c.add_points(points_2 / ((2**15) / 16.))
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
