y = unpackd[1] # y+ -> pitch+
z = unpackd[2] # z+ -> thrust+
#x = 1000*unpackd[0] # x+ -> roll-
#y = 1000*unpackd[1] # y+ -> pitch+
#z = 1000*unpackd[2] # z+ -> thrust+
""" Rotate Optitrack Axis"""
#y, z = z, y
"""
Orientation Feedback: quaternion given as (qx, qy, qz, qw)
"""
qx, qy, qz, qw = unpackd[3], unpackd[4], unpackd[5], unpackd[6]
q = np.array([qx, qy, qz, qw])
q = np.linalg.norm(q)
#print("X:{}, Y:{}, Z:{}".format(x, y, z))
orientation = euler_from_quaternion([unpackd[3], unpackd[4], unpackd[5], unpackd[6]], axes='sxzy')
#orientation = [elem*(180/math.pi) for elem in orientation]
roll = orientation[1]
pitch = orientation[0]
yaw = orientation[2]
"""
Check if vehicle is being tracked by cameras
"""
detected = unpackd[-1]
if detected:
last_detect_ts = time.time()
delta = unpackd[-2]
# z+ -> thrust+
x = unpackd[0]
y = unpackd[1]
z = unpackd[2]
# Swap Z and Y axes since Y axis is 'up' w/ Optitrack
y, z = -z, y
# Orientation Feedback: quaternion given as (qx, qy, qz, qw)
qx, qy, qz, qw = unpackd[3], unpackd[4], unpackd[5], unpackd[6]
q = np.array([qx, qy, qz, qw])
np.linalg.norm(q)
#print(q)
#print("X:{}, Y:{}, Z:{}".format(x, y, z))
orientation = euler_from_quaternion(q, axes='syxz')
orientation = [elem*(180/math.pi) for elem in orientation]
yaw = orientation[0]
roll = orientation[1]
pitch = orientation[2]
"""
Check if body is being tracked by cameras
"""
detected = unpackd[-1]
if detected:
detect_ts = int(round(time.time() * 1000))
delta = unpackd[-2]
else:
from feedback.transformations import euler_from_quaternion
import time
context = zmq.Context()
optitrack_conn = context.socket(zmq.REP)
optitrack_conn.bind("tcp://204.102.224.3:5000")
while 1:
packet = optitrack_conn.recv()
filtered = msgpack.unpackb(packet)
#print "Got", filtered[3], filtered[4], filtered[5], filtered[6]
optitrack_conn.send(b'Ack', zmq.NOBLOCK)
EulerAngles = namedtuple('EulerAngles', 'pitch roll yaw')
res = euler_from_quaternion([filtered[3], filtered[4], filtered[5], filtered[6]], axes='syxz')
res = [elem*(180/math.pi) for elem in res]
print filtered[0], filtered[1], filtered[2], res
#time.sleep(.5)
#print res, filtered[-1], filtered[-2]
"""
Rbn = np.matrix([[q[1]**2 + q[2]**2 - q[3]**2 - q[4]**2, 2*(q[2] * q[3] - q[1] * q[4]), 2*(q[1] * q[3] + q[2] * q[4])],
[2*(q[2] * q[3] + q[1] * q[4]), q[1]**2 - q[2]**2 + q[3]**2 - q[4]**2, 2*(q[3] * q[4] - q[1] * q[2])],
[2*(q[2] * q[4] - q[1] * q[3]), 2*(q[1] * q[2] + q[3] * q[4]), q[1]**2 - q[2]**2 - q[3]**2 + q[4]**2]])
R = np.transpose(Rbn)
phi = math.atan2(R[1, 2], R[2, 2])*(180/math.pi)
theta = -math.asin(R[0, 2])*(180/math.pi)
psi = -math.atan2(R[0, 1], R[0, 0])*(180/math.pi)
