Position / velocity tracking

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_B2(t, y):
    body = y[:12]
    pos_int = y[12:18]
    att_int = y[18:24]

    sp = HOVER_SP.copy()
    sp[0] = 5.0 * min(t / 30.0, 1.0)

    pos_err = sp[:3] - body[:3]
    vel_err = sp[3:] - body[3:6]

    ax_d = KP_POS * pos_err[0] + KD_POS * vel_err[0] + _KI_POS * pos_int[0]
    ay_d = KP_POS * pos_err[1] + KD_POS * vel_err[1] + _KI_POS * pos_int[1]
    az_d = KP_POS * pos_err[2] + KD_POS * vel_err[2] + _KI_POS * pos_int[2]

    psi = body[8]
    T_des = MASS * (G + az_d)
    phi_des = (1.0 / G) * (ax_d * np.sin(psi) - ay_d * np.cos(psi))
    theta_des = (1.0 / G) * (ax_d * np.cos(psi) + ay_d * np.sin(psi))

    phi, theta = body[6], body[7]
    p, q, r_rate = body[9], body[10], body[11]
    e_att = np.array([phi_des - phi, theta_des - theta, -psi])
    tau = KP_ATT * e_att - KD_ATT * np.array([p, q, r_rate]) + _KI_ATT * att_int[:3]
    tau = np.clip(tau, -TORQUE_CLIP, TORQUE_CLIP)
    T_des = np.clip(T_des, 0.0, THRUST_MAX)

    d_body = _quad12(body, T_des, tau[0], tau[1], tau[2])
    d_pos_int = np.clip(np.concatenate([pos_err, vel_err]), -_KI_POS_CLIP, _KI_POS_CLIP)
    d_att_int = np.clip(np.concatenate([e_att, -np.array([p, q, r_rate])]), -_INT_CLIP, _INT_CLIP)

    return np.concatenate([d_body, d_pos_int, d_att_int])