def test_angle():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(rand_q(8))
q0 = normalized[0]
for d in range(normalized.shape[0]):
q1 = normalized[d]
ang1 = tq.min_angle(q0, q1)
ang2 = tq.min_angle(q1, q0)
assert torch.abs(ang1 - ang2) < 1e-6, ang1 - ang2
normalized2 = tq.normalize(torch.Tensor(myq))
angles = tq.min_angle(normalized, normalized2)
for d in range(normalized.shape[0]):
ang0 = tq.min_angle(normalized[d], normalized2[d])
assert torch.abs(angles[d] - ang0.double()) < 1e-7, \
angles[d] - ang0.double()
for d in range(normalized.shape[0]):
ang0 = pmath.angle_between_quaternions(normalized[d].numpy(),
normalized2[d].numpy())
assert np.all(np.abs(angles[d].numpy() - ang0) < 1e-6), \
angles[d].numpy() - ang0
def update_alignment(self, q): a2 = 2*math.atan2(q[3], q[0]) heading_offset = a2*180/math.pi off = self.heading_off.value - heading_offset o = quaternion.angvec2quat(off*math.pi/180, [0, 0, 1]) self.alignmentQ.update(quaternion.normalize(quaternion.multiply(q, o))) self.auto_cal.SetNorm(quaternion.rotvecquat([0, 0, 1], self.alignmentQ.value))
def test_multiply():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(torch.Tensor(myq))
normalized2 = tq.normalize(rand_q(8))
multiplied = tq.multiply(normalized, normalized2)
for d in range(normalized.shape[0]):
mul0 = tq.multiply(normalized[d], normalized2[d])
assert torch.all(multiplied[d] == mul0)
for d in range(normalized.shape[0]):
mul0 = pmath.quaternion_multiply(normalized[d].numpy(),
normalized2[d].numpy())
assert np.all(multiplied[d].numpy() == mul0)
def test_convert():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(torch.Tensor(rand_q(8)))
translation = torch.rand([8, 3])
rot0, trans0 = tq.posenetQT_to_opensfmRT(normalized[0], translation[0])
rotations, translations = tq.posenetQT_to_opensfmRT(
normalized, translation)
for d in range(normalized.shape[0]):
rot0, trans0 = tq.posenetQT_to_opensfmRT(normalized[d], translation[d])
assert torch.all(rotations[d] == rot0),\
torch.max(rotations[d] - rot0)
assert torch.all(translations[d] == trans0),\
torch.max(translations[d] - trans0)
for d in range(normalized.shape[0]):
rot0, trans0 = pmath.obtain_opensfmRT_from_posenetQT(
normalized[d].numpy(), translation[d].numpy())
rot0 = torch.Tensor(rot0)
trans0 = torch.Tensor(trans0)
assert torch.abs(torch.max(rotations[d] - rot0)) < 1e-6,\
torch.max(rotations[d] - rot0)
assert torch.abs(torch.max(translations[d] - trans0)) < 1e-6,\
torch.max(translations[d] - trans0)
def rate_to_quat(self, omega, dt):
"""Rotation quaternion from gyroscope sample
Args:
omega (numpy.ndarray): angular velocity vector [x,y,z]. Same as scaled gyro sample in rad/s.
dt (float): Time delta between gyro samples for angle integration.
Returns:
numpy.ndarray: Rotation quaternion corresponding to orientation change
"""
# https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity/24201879#24201879
# no idea how it fully works, but it does
ha = omega * dt * 0.5
l = np.sqrt(ha.dot(ha))
if l > 1.0e-12:
ha *= np.sin(l) / l
q0 = np.cos(l)
q1 = ha[0]
q2 = ha[1]
q3 = ha[2]
return quat.normalize(quat.quaternion(q0, q1, q2, q3))
else:
return quat.quaternion(1, 0, 0, 0)
def get_vector_from_quat(quaternion):
qw, qx, qy, qz = quaternion
if qw > 1:
quaternion.normalize(quaternion)
qw, qx, qy, qz = quaternion
angle = 2 * math.acos(qw)
angle = math.degrees(angle)
s = math.sqrt(1 - qw * qw)
if s < 0.001:
x = qx
y = qy
z = qz
else:
x = qx / s
y = qy / s
z = qz / s
vector = angle, x, y, z
return vector
def test_normalize():
normalized = tq.normalize(torch.Tensor(myq))
for d in range(myq.shape[0]):
assert torch.all(normalized[d] == tq.normalize(torch.Tensor(myq[d])))
for d in range(myq.shape[0]):
iq = pmath.normalize_quaternion(myq[d])
mq = normalized[d].numpy()
assert np.all(np.abs(mq - iq) < 1e-7), normalized[d] - iq
random_q = rand_q()
normalized_rand = tq.normalize(random_q)
for d in range(random_q.shape[0]):
iq = pmath.normalize_quaternion(random_q[d].numpy())
mq = normalized_rand[d].numpy()
assert np.all(np.abs(mq - iq) < 1e-6), mq - iq
def test_conjugate():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(torch.Tensor(rand_q(8)))
con = tq.conjugate(normalized)
for d in range(normalized.shape[0]):
con0 = tq.conjugate(normalized[d])
assert torch.all(con[d] == con0)
for d in range(normalized.shape[0]):
con0 = pmath.quaternion_conjugate(normalized[d].numpy())
assert np.all(con[d].numpy() == con0)
def integrate_all(self):
"""go through each sample and integrate to find orientation. Assumes sample N contains change between N and N-1
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated:
return (self.time_list, self.orientation_list)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(self.data[0][0] - 1)
for i in range(self.num_data_points):
# angular velocity vector
omega = self.data[i][1:]
# get current time
this_time = self.data[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = 1 # frame
# Only calculate if angular velocity is present
if np.any(omega):
# calculate rotation quaternion
delta_q = self.rate_to_quat(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quat.quaternion_multiply(self.orientation, delta_q) # Maybe change order
self.orientation = quat.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
def integrate_all(self):
"""go through each gyro sample and integrate to find orientation
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated:
return (self.time_list, self.orientation_list)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
for i in range(self.num_data_points):
# angular velocity vector
omega = self.data[i][1:]
# get current and adjecent times
last_time = self.data[i - 1][0] if i > 0 else self.data[i][0]
this_time = self.data[i][0]
next_time = self.data[
i + 1][0] if i < self.num_data_points - 1 else self.data[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = (next_time - last_time) / 2
# Only calculate if angular velocity is present
if np.any(omega):
# calculate rotation quaternion
delta_q = self.rate_to_quart(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quart.quaternion_multiply(
self.orientation, delta_q) # Maybe change order
self.orientation = quart.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
def test_rotation():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(torch.Tensor(rand_q(8)))
con0 = tq.get_rotation(normalized[0])
con = tq.get_rotation(normalized)
for d in range(normalized.shape[0]):
con0 = tq.get_rotation(normalized[d])
assert torch.all(con[d] == con0)
for d in range(normalized.shape[0]):
con0 = pmath.quaternion_to_rotation_matrix(normalized[d].numpy())
assert np.abs(np.max(con[d].numpy() - con0)) < 1e-7, \
np.max(con[d].numpy() - con0)
def convolve_quaternion(im1, im2, name, mode=0):
im1 /= 255.0
im1 = quaternion.pad(im1)
im2 /= 255.0
im2 = quaternion.pad(im2)
qfunc = quaternion.AQCV2
if mode in (1, 2, 3):
qfunc = quaternion.SPQCV
r, i, j, k = qfunc(np.zeros(im1.shape[:2]),
im1[:, :, 0], im1[:, :, 1], im1[:, :, 2],
np.zeros(im2.shape[:2]), im2[:, :, 0], im2[:, :, 1],
im2[:, :, 2], mode)
else:
r, i, j, k = qfunc(0, im1[:, :, 0], im1[:, :, 1], im1[:, :, 2], mode)
img = quaternion.create_image(r, i, j, k)
img = quaternion.sqrt_normalize(img)
for i in range(3):
img[:, :, i] = np.multiply(img[:, :, i], img[:, :, 3])
img = quaternion.normalize(img[:, :, :3])
img *= 255.1
imsave(name, img.astype(np.uint8))
return img
def read(self):
data = self.IMUread()
if not data:
if time.monotonic(
) - self.last_imuread > 1 and self.frequency.value:
print('IMURead failed!')
self.frequency.set(False)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
self.last_imuread = time.monotonic()
self.frequency.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
aligned = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(aligned))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .01 and dt < .2:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose, aligned))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or \
self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
if self.auto_cal.cal_pipe:
#print('warning, cal pipe always sending despite locks')
cal_data = {}
#how to check this here?? if not 'imu.accel.calibration.locked'
cal_data['accel'] = list(data['accel'])
#how to check this here?? if not 'imu.compass.calibration.locked'
cal_data['compass'] = list(data['compass'])
cal_data['fusionQPose'] = list(data['fusionQPose'])
if cal_data:
#print('send', cal_data)
self.auto_cal.cal_pipe.send(cal_data)
return data
o_estimate[:, 0] = o[:, 0]
for i in xrange(1, len(time)):
# prediction
F = Jf(x, dt[i-1])
x = f(x, dt[i-1])
P = F.dot(P).dot(F.T) + Q
o_prediction[:, [i]] = extract(x)[0]
# estimate
correction = z[:, [i]] - H.dot(x)
K = P.dot(H.T).dot(np.linalg.inv(H.dot(P).dot(H.T) + R))
x = x + K.dot(correction)
x[:o.shape[0]] = quat.normalize(x[:o.shape[0]])
P = P - K.dot(H).dot(P)
# estimate
correction = prime[:, [i]] - Hp.dot(x)
K = P.dot(Hp.T).dot(np.linalg.inv(Hp.dot(P).dot(Hp.T) + Rp))
x = x + K.dot(correction)
x[:o.shape[0]] = quat.normalize(x[:o.shape[0]])
P = P - K.dot(Hp).dot(P)
o_estimate[:, [i]] = extract(x)[0]
o, o_prediction, o_estimate = (quat.to_euler(i) for i in (o, o_prediction, o_estimate))
prime = quat.to_euler(prime)
# prime[0, :], prime[1, :] = prime[1, :].copy(), prime[0, :].copy()
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print 'IMURead failed!'
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print 'vector n', data['accel']
return False
self.last_imuread = time.time()
self.loopfreq.strobe()
if not self.FirstTimeStamp:
self.FirstTimeStamp = data['timestamp']
data['timestamp'] -= self.FirstTimeStamp
#data['accel_comp'] = quaternion.rotvecquat(vector.sub(data['accel'], down), self.alignmentQ.value)
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
self.server.TimeStamp('imu', data['timestamp'])
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
compass, accel, down = False, False, False
if not self.accel_calibration.locked.value:
accel = list(data['accel'])
if not self.compass_calibration.locked.value:
down = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(origfusionQPose))
compass = list(data['compass']) + down
if accel or compass:
self.auto_cal.AddPoint((accel, compass, down))
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
if self.heading_off.value != self.last_heading_off:
self.update_alignment(self.alignmentQ.value)
self.last_heading_off = self.heading_off.value
result = self.auto_cal.UpdatedCalibration()
if result:
if result[0] == 'accel' and not self.accel_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.accel_calibration.sigmapoints.set(result[2])
if result[1]:
self.accel_calibration.age.reset()
self.accel_calibration.set(result[1])
elif result[
0] == 'compass' and not self.compass_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.compass_calibration.sigmapoints.set(result[2])
if result[1]:
self.compass_calibration.age.reset()
self.compass_calibration.set(result[1])
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def set(self, value):
if value:
value = quaternion.normalize(value)
super(QuaternionValue, self).set(value)
def integrate_all(self, use_acc=False):
"""go through each gyro sample and integrate to find orientation
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated and (use_acc == self.last_used_acc
or not self.acc_available):
return (self.time_list, self.orientation_list)
apply_complementary = self.acc_available and use_acc
self.last_used_acc = use_acc
if apply_complementary:
# find valid accelation data points
#print(self.acc)
#print(self.acc.shape)
asquared = np.sum(self.acc[:, 1:]**2, 1)
# between 0.9 and 1.1 g
complementary_mask = np.logical_and(0.81 < asquared,
asquared < 1.21)
self.orientation = np.copy(self.initial_orientation)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
start_time = self.gyro[0][0] # seconds
for i in range(self.num_data_points):
# angular velocity vector
omega = self.gyro[i][1:]
# get current and adjecent times
last_time = self.gyro[i - 1][0] if i > 0 else self.gyro[i][0]
this_time = self.gyro[i][0]
next_time = self.gyro[
i + 1][0] if i < self.num_data_points - 1 else self.gyro[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = (next_time - last_time) / 2
# Only calculate if angular velocity is present
if np.any(omega) or apply_complementary:
# complementary filter
if apply_complementary:
if complementary_mask[i]:
avec = self.acc[i][1:]
avec /= np.linalg.norm(avec)
accWorldVec = quat.rotate_vector_fast(
self.orientation, avec)
correctionWorld = np.cross(accWorldVec, self.grav_vec)
# high weight for first two seconds to "lock" it, then
weight = 10 if this_time - start_time < 1.5 else 0.6
correctionBody = weight * quat.rotate_vector_fast(
quat.conjugate(self.orientation), correctionWorld)
omega = omega + correctionBody
# calculate rotation quaternion
delta_q = self.rate_to_quat(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quat.quaternion_multiply(
self.orientation, delta_q) # Maybe change order
self.orientation = quat.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
for i in xrange(1, len(t)):
# prediction
F = Jf(x, dt)
x = f(x, dt)
# x = F.dot(x)
P = F.dot(P).dot(F.T) + Q
o_prediction[:, [i]] = extract(x)[0]
# estimate
correction = z[:, [i]] - H.dot(x)
K = P.dot(H.T).dot(np.linalg.inv(H.dot(P).dot(H.T) + R))
x = x + K.dot(correction)
x[:o.shape[0]] = quat.normalize(x[:o.shape[0]])
P = P - K.dot(H).dot(P)
o_estimate[:, [i]] = extract(x)[0]
o, o_noisy, o_prediction, o_estimate = (quat.to_euler(i)
for i in (o, o_noisy, o_prediction,
o_estimate))
# o_noisy[:2, :] = o_prediction[:2, :] = o_estimate[:2, :] = o[:2, :]
mse_noise, mse_prediction, mse_estimate = (mse(o, i)
for i in (o_noisy, o_prediction,
o_estimate))
print 'noisy error:', mse_noise
print 'prediction error:', mse_prediction
print 'estimate error:', mse_estimate
def set(self, value): super(QuaternionValue, self).set(quaternion.normalize(value))
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print('IMURead failed!')
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
t = time.time()
self.timestamp.set(t - self.starttime)
self.last_imuread = t
self.loopfreq.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
cal_data = {}
if not self.accel_calibration.locked.value:
cal_data['accel'] = list(data['accel'])
if not self.compass_calibration.locked.value:
cal_data['compass'] = list(data['compass'])
cal_data['down'] = quaternion.rotvecquat(
[0, 0, 1], quaternion.conjugate(origfusionQPose))
if cal_data:
self.auto_cal.cal_pipe.send(cal_data)
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
