INNER CODE UNIT · Python
_quaternion_from_euler_zyx
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:74
def _quaternion_from_euler_zyx(roll, pitch, yaw):
"""ZYX Euler -> (x, y, z, w)."""
cy, sy = math.cos(yaw / 2), math.sin(yaw / 2)
cp, sp = math.cos(pitch / 2), math.sin(pitch / 2)
cr, sr = math.cos(roll / 2), math.sin(roll / 2)
return (sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy)
def _imu_quat_from_enu_yaw(yaw_enu, pitch=0.0, roll=0.0):
"""
Convert ENU yaw to IMU quaternion for imu_attitude_azimuth_offset_deg=0.
The smoother recovers ENU yaw as: ENU_yaw = quat_yaw + pi/2.
So we must publish: quat_yaw = ENU_yaw - pi/2.
"""