def __init__(self):
self.attitude_quaternion = euclid3.Quaternion(1.0, 0, 0, 0)
self.attitude_euler = self.attitude_quaternion.get_euler()
self.temperature = 0
def get_acc_earth(self):
q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3)
a = euclid3.Vector3(self.ax, self.ay, self.az)
e = q * a
return euclid3.Vector3(e.x, e.y, e.z)
def get_euler(self):
q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3)
euler = q.get_euler()
return euclid3.Vector3(euler[2] * 57.29577951, euler[0] * 57.29577951,
euler[1] * 57.29577951)
def get_quat(self):
return euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3)
import euclid3 q = euclid3.Quaternion(1, 0.5, 0, 0) euler = q.get_euler() print(q) print(euler[1]) print(type(euler[1]))
print("IMU Init Failed")
sys.exit(1)
else:
print("IMU Init Succeeded")
# this is a good time to set any fusion parameters
imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
poll_interval = imu.IMUGetPollInterval()
print("Recommended Poll Interval: %dmS\n" % poll_interval)
q = euclid3.Quaternion()
while True:
if imu.IMURead():
# x, y, z = imu.getFusionData()
# print("%f %f %f" % (x,y,z))
data = imu.getIMUData()
fusionPose = data["fusionQPose"]
q.w = fusionPose[0]
q.x = fusionPose[1]
q.y = fusionPose[2]
q.z = fusionPose[3]
print(math.atan2(2*q.y*q.w-2*q.x*q.z,1-2*q.y*q.y -2*q.z*q.z))
# print(math.asin(2*(q.x * q.y + q.w * q.z)))
# print(q.w,q.x,q.y,q.z)
# print(q.get_euler())