INNER CODE UNIT · Python
sr
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:78
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.
"""
return _quaternion_from_euler_zyx(roll, pitch, yaw_enu - math.pi / 2)
def _enu_to_latlon(east, north, alt,