def DMP_get_euler(self, a_quat):
psi = math.atan2(2 * a_quat.x * a_quat.y - 2 * a_quat.w * a_quat.z,
2 * a_quat.w * a_quat.w + 2 * a_quat.x * a_quat.x - 1)
theta = -math.asin(2 * a_quat.x * a_quat.z + 2 * a_quat.w * a_quat.y)
phi = math.atan2(2 * a_quat.y * a_quat.z - 2 * a_quat.w * a_quat.x,
2 * a_quat.w * a_quat.w + 2 * a_quat.z * a_quat.z - 1)
return V(psi, theta, phi)
def DMP_get_roll_pitch_yaw(self, a_quat, a_grav_vect):
# roll: (tilt left/right, about X axis)
roll = math.atan(a_grav_vect.y /
math.sqrt(a_grav_vect.x * a_grav_vect.x +
a_grav_vect.z * a_grav_vect.z))
# pitch: (nose up/down, about Y axis)
pitch = math.atan(a_grav_vect.x /
math.sqrt(a_grav_vect.y * a_grav_vect.y +
a_grav_vect.z * a_grav_vect.z))
# yaw: (about Z axis)
yaw = math.atan2(2 * a_quat.x * a_quat.y - 2 * a_quat.w * a_quat.z,
2 * a_quat.w * a_quat.w + 2 * a_quat.x * a_quat.x - 1)
return V(roll, pitch, yaw)
def DMP_get_roll_pitch_yaw(self, a_quat, a_grav_vect):
# roll: (tilt left/right, about X axis)
#roll = math.atan(a_grav_vect.y /
# math.sqrt(a_grav_vect.x*a_grav_vect.x +
# a_grav_vect.z*a_grav_vect.z))
# pitch: (nose up/down, about Y axis)
#pitch = math.atan(a_grav_vect.x /
# math.sqrt(a_grav_vect.y*a_grav_vect.y +
# a_grav_vect.z*a_grav_vect.z))
# yaw: (about Z axis)
yaw = 2 * math.atan2(
(2.0 * a_quat.x * a_quat.y - 2.0 * a_quat.w * a_quat.z),
(2.0 * a_quat.w * a_quat.w + 2.0 * a_quat.x * a_quat.x - 1.0))
return V(0, 0, yaw)
def DMP_get_euler_roll_pitch_yaw(self, a_quat, a_grav_vect):
rad_ypr = self.DMP_get_roll_pitch_yaw(a_quat, a_grav_vect)
roll = rad_ypr.x * (180.0 / math.pi)
pitch = rad_ypr.y * (180.0 / math.pi)
yaw = rad_ypr.z * (180.0 / math.pi)
return V(roll, pitch, yaw)
def DMP_get_linear_accel(self, a_vector_raw, a_vect_grav):
x = a_vector_raw.x - a_vect_grav.x * 8192
y = a_vector_raw.y - a_vect_grav.y * 8192
z = a_vector_raw.z - a_vect_grav.z * 8192
return V(x, y, z)
def DMP_get_linear_accel_int16(self, a_v_raw, a_grav):
x = ctypes.c_int16(a_v_raw.x - (a_grav.x * 8192)).value
y = ctypes.c_int16(a_v_raw.y - (a_grav.y * 8192)).value
z = ctypes.c_int16(a_v_raw.y - (a_grav.y * 8192)).value
return V(x, y, z)
def DMP_get_gravity(self, a_quat):
x = 2.0 * (a_quat.x * a_quat.z - a_quat.w * a_quat.y)
y = 2.0 * (a_quat.w * a_quat.x + a_quat.y * a_quat.z)
z = 1.0 * (a_quat.w * a_quat.w - a_quat.x * a_quat.x -
a_quat.y * a_quat.y + a_quat.z * a_quat.z)
return V(x, y, z)
def DMP_get_acceleration_int16(self, a_FIFO_buffer):
x = ctypes.c_int16(a_FIFO_buffer[28] << 8 | a_FIFO_buffer[29]).value
y = ctypes.c_int16(a_FIFO_buffer[32] << 8 | a_FIFO_buffer[33]).value
z = ctypes.c_int16(a_FIFO_buffer[36] << 8 | a_FIFO_buffer[37]).value
return V(x, y, z)
