6DOF Quadrotor Rigid Body (12-state)

def _motor_speeds(t: float) -> np.ndarray:
    """Motor angular velocities for a typical test manoeuvre."""
    w_hover = math.sqrt(_MASS * _G / (4.0 * _Kf))

    if t < 2.0:
        # Hover
        return np.array([w_hover, w_hover, w_hover, w_hover])
    elif t < 5.0:
        # Pitch forward: increase rear, decrease front
        delta = 0.03 * w_hover
        return np.array([w_hover - delta, w_hover + delta,
                         w_hover - delta, w_hover + delta])
    elif t < 8.0:
        # Yaw right: differential CW/CCW
        delta = 0.02 * w_hover
        return np.array([w_hover + delta, w_hover + delta,
                         w_hover - delta, w_hover - delta])
    elif t < 12.0:
        # Return to hover
        return np.array([w_hover, w_hover, w_hover, w_hover])
    else:
        # Slow descent
        return np.array([w_hover * 0.92, w_hover * 0.92,
                         w_hover * 0.92, w_hover * 0.92])

def rhs_6dof_quad(t, y):
    """12-state rigid-body 6DOF quadrotor dynamics.

    State vector: [x, y, z, u, v, w, phi, theta, psi, p, q, r]
    """
    dy = np.zeros(12)

    u, v, w = y[3], y[4], y[5]
    phi, theta, psi = y[6], y[7], y[8]
    p, q, r = y[9], y[10], y[11]

    # Motor forces and torques
    omega = _motor_speeds(t)
    F = _Kf * omega**2
    M = _Km * omega**2

    T_total = np.sum(F)

    # Torques from X-frame motor layout
    L_roll = _L * (-F[0] + F[1] + F[2] - F[3])
    M_pitch = _L * (-F[0] - F[1] + F[2] + F[3])
    N_yaw = np.sum(_SPIN_DIRS * M)

    # Aero drag (body-frame, linear approximation)
    V_body = math.sqrt(u**2 + v**2 + w**2)
    if V_body > 0.01:
        Fd_x = -_CD_BODY * 0.5 * 1.225 * V_body * u
        Fd_y = -_CD_BODY * 0.5 * 1.225 * V_body * v
        Fd_z = -_CD_BODY * 0.5 * 1.225 * V_body * w
    else:
        Fd_x = Fd_y = Fd_z = 0.0

    # Rotation matrix elements (ZYX convention: psi, theta, phi)
    cphi, sphi = math.cos(phi), math.sin(phi)
    cth, sth = math.cos(theta), math.sin(theta)
    cpsi, spsi = math.cos(psi), math.sin(psi)

    # Position derivatives (NED frame)
    dy[0] = (cth * cpsi) * u + (sphi * sth * cpsi - cphi * spsi) * v + \
            (cphi * sth * cpsi + sphi * spsi) * w
    dy[1] = (cth * spsi) * u + (sphi * sth * spsi + cphi * cpsi) * v + \
            (cphi * sth * spsi - sphi * cpsi) * w
    dy[2] = (-sth) * u + (sphi * cth) * v + (cphi * cth) * w

    # Velocity derivatives (body frame, Newton's 2nd law)
    dy[3] = (r * v - q * w) + Fd_x / _MASS - _G * sth
    dy[4] = (p * w - r * u) + Fd_y / _MASS + _G * cth * sphi
    dy[5] = (q * u - p * v) + Fd_z / _MASS + _G * cth * cphi - T_total / _MASS

    # Euler angle derivatives (kinematic equations)
    if abs(cth) > 1e-6:
        dy[6] = p + (sphi * q + cphi * r) * sth / cth
        dy[7] = cphi * q - sphi * r
        dy[8] = (sphi * q + cphi * r) / cth
    else:
        dy[6] = p
        dy[7] = cphi * q - sphi * r
        dy[8] = 0.0

    # Angular rate derivatives (Euler's equations)
    dy[9] = (L_roll - ((_Izz - _Iyy) * q * r)) / _Ixx
    dy[10] = (M_pitch - ((_Ixx - _Izz) * p * r)) / _Iyy
    dy[11] = (N_yaw - ((_Iyy - _Ixx) * p * q)) / _Izz

    return dy