INNER CODE UNIT · Python

ENU_yaw

MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:89

    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)


def _imu_linear_acc_body(yaw, pitch, roll, ax_world=0.0, ay_world=0.0):
    """Specific force in body frame (m/s²): (world_accel - gravity) rotated to body."""
    sf = np.array([ax_world, ay_world, 9.81]
                  )   # specific force in world (up = +Z)

View source record →