INNER CODE UNIT · Python
_imu_quat_from_enu_yaw
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:85
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,
lat0=_GT_LAT0_DEG, lon0=_GT_LON0_DEG, alt0=_GT_ALT0_M):
m_per_deg_lon = _M_PER_DEG_LAT * math.cos(math.radians(lat0))
return (lat0 + north / _M_PER_DEG_LAT,
lon0 + east / max(m_per_deg_lon, 1e-9),
alt0 + alt)