Attitude stabilization (closed-loop)

def _body_forces(T, phi, theta, psi):
    """Thrust-to-inertial force components."""
    cp, sp = np.cos(phi), np.sin(phi)
    ct, st = np.cos(theta), np.sin(theta)
    cy, sy = np.cos(psi), np.sin(psi)
    Fx = T * (cy * st * cp + sy * sp)
    Fy = T * (sy * st * cp - cy * sp)
    Fz = T * ct * cp
    return Fx, Fy, Fz

def _euler_kinematics(phi, theta, p, q, r):
    """Euler-angle rates from body rates. Returns (dphi, dtheta, dpsi)."""
    cp, sp = np.cos(phi), np.sin(phi)
    theta_c = np.clip(theta, -1.39, 1.39)
    tan_th = np.tan(theta_c)
    cos_th = np.cos(theta_c)
    sec_th = 1.0 / cos_th if abs(cos_th) > 1e-12 else 1e12 * np.sign(cos_th)
    dphi = p + q * sp * tan_th + r * cp * tan_th
    dtheta = q * cp - r * sp
    dpsi = (q * sp + r * cp) * sec_th
    return dphi, dtheta, dpsi

def _quad12(y, T, tau_x, tau_y, tau_z, mass=None):
    """Core 12-state quadrotor dynamics. Returns d[0:12].

    BA-2 (2026-04-29): added optional ``mass`` kwarg so the
    factory variants can override the module-global ``MASS`` for the
    translational acceleration / drag terms.
    """
    eff_mass = MASS if mass is None else mass
    phi, theta, psi = y[6], y[7], y[8]
    p, q, r = y[9], y[10], y[11]
    Fx, Fy, Fz = _body_forces(T, phi, theta, psi)

    d = np.empty(12)
    d[0] = y[3]; d[1] = y[4]; d[2] = y[5]
    d[3] = (Fx - CD * y[3]) / eff_mass
    d[4] = (Fy - CD * y[4]) / eff_mass
    d[5] = (Fz - CD * y[5]) / eff_mass - G
    d[6], d[7], d[8] = _euler_kinematics(phi, theta, p, q, r)
    d[9] = (tau_x + (IYY - IZZ) * q * r) / IXX
    d[10] = (tau_y + (IZZ - IXX) * p * r) / IYY
    d[11] = (tau_z + (IXX - IYY) * p * q) / IZZ
    return d

def rhs_B1(t, y):
    body = y[:12]
    integ = y[12:18]

    phi, theta, psi = body[6], body[7], body[8]
    p, q, r_rate = body[9], body[10], body[11]

    T_cmd = MASS * G
    e_att = np.array([0.0 - phi, 0.0 - theta, 0.0 - psi])
    e_rate = np.array([0.0 - p, 0.0 - q, 0.0 - r_rate])

    tau = (KP_ATT * e_att + KD_ATT * (-np.array([p, q, r_rate]))
           + _KI_ATT * integ[:3] + _KI_RATE * integ[3:])
    tau = np.clip(tau, -TORQUE_CLIP, TORQUE_CLIP)

    d_body = _quad12(body, T_cmd, tau[0], tau[1], tau[2])
    d_int_att = np.clip(e_att, -_INT_CLIP, _INT_CLIP)
    d_int_rate = np.clip(e_rate, -_INT_CLIP, _INT_CLIP)

    return np.concatenate([d_body, d_int_att, d_int_rate])