Padé-delayed attitude feedback

def _pade2_coeffs(tau):
    """2nd-order Padé: (1 - s*tau/2 + s²*tau²/12) / (1 + s*tau/2 + s²*tau²/12)."""
    a1 = tau / 2.0
    a2 = tau**2 / 12.0
    return a1, a2

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_B6(t, y):
    body = y[:12]
    delay_states = y[12:22].reshape(5, 2)

    delayed_feedback = np.zeros(5)
    d_delay = np.zeros((5, 2))
    channels = [body[6], body[7], body[8], body[9], body[10]]  # phi,theta,psi,p,q

    for i in range(5):
        a1, a2 = _pade2_coeffs(_PADE_DELAYS[i])
        x1, x2 = delay_states[i]
        u = channels[i]
        if a2 > 1e-15:
            d_delay[i, 0] = x2
            d_delay[i, 1] = (u - x1 - a1 * x2) / a2
        else:
            d_delay[i, 0] = (u - x1) / max(a1, 1e-12)
            d_delay[i, 1] = 0.0
        delayed_feedback[i] = x1

    phi_d, theta_d, psi_d = delayed_feedback[0], delayed_feedback[1], delayed_feedback[2]
    p_d, q_d = delayed_feedback[3], delayed_feedback[4]
    r_rate = body[11]

    T_cmd = MASS * G
    tau_x = KP_ATT * (-phi_d) - KD_ATT * p_d
    tau_y = KP_ATT * (-theta_d) - KD_ATT * q_d
    tau_z = KP_YAW * (-psi_d) - KD_YAW * r_rate
    tau_x = np.clip(tau_x, -TORQUE_CLIP, TORQUE_CLIP)
    tau_y = np.clip(tau_y, -TORQUE_CLIP, TORQUE_CLIP)
    tau_z = np.clip(tau_z, -TORQUE_CLIP * 0.25, TORQUE_CLIP * 0.25)

    d_body = _quad12(body, T_cmd, tau_x, tau_y, tau_z)
    return np.concatenate([d_body, d_delay.ravel()])