Quad BLDC Motor DQ-Frame PMSM (20-state)

def _omega_target(motor_idx: int, t: float) -> float:
    """Target speed with step perturbation."""
    base = _OMEGA_TARGETS[motor_idx]
    if t > _STEP_TIMES[motor_idx]:
        base += _STEP_MAGNITUDES[motor_idx]
    return base

def _motor_rhs(y_motor, t, motor_idx):
    """Single PMSM motor dynamics in DQ frame."""
    i_d = y_motor[0]
    i_q = y_motor[1]
    omega_m = y_motor[2]
    theta_e = y_motor[3]
    t_wind = y_motor[4]

    omega_e = _P * omega_m

    # Temperature-dependent resistance
    r_s = _R_S_20 * (1.0 + _ALPHA_CU * (t_wind - 20.0))

    # Speed controller: feedforward + proportional
    omega_ref = _omega_target(motor_idx, t)
    speed_error = omega_ref - omega_m
    v_q_cmd = _LAMBDA_M * omega_e + _KP_SPEED * speed_error
    v_d_cmd = 0.0  # field-oriented: i_d* = 0

    # Voltage saturation (bus limit via space-vector modulus)
    v_mag = np.sqrt(v_d_cmd**2 + v_q_cmd**2)
    v_max = _V_BUS / np.sqrt(3.0)
    if v_mag > v_max:
        scale = v_max / v_mag
        v_d_cmd *= scale
        v_q_cmd *= scale

    # DQ electrical dynamics
    di_d = (v_d_cmd - r_s * i_d + omega_e * _L_Q * i_q) / _L_D
    di_q = (v_q_cmd - r_s * i_q - omega_e * (_L_D * i_d + _LAMBDA_M)) / _L_Q

    # Electromagnetic torque
    t_em = 1.5 * _P * (_LAMBDA_M * i_q + (_L_D - _L_Q) * i_d * i_q)

    # Cogging torque
    t_cog = _T_COG_MAX * np.sin(_N_COG * theta_e)

    # Aerodynamic load torque
    t_load = _K_TORQUE * omega_m * abs(omega_m)

    # Mechanical dynamics
    domega = (t_em + t_cog - t_load - _B_FRICTION * omega_m) / _J_ROTOR
    dtheta = omega_e

    # Thermal dynamics
    p_copper = 1.5 * r_s * (i_d**2 + i_q**2)
    r_th_total = _R_TH_WIND_CASE + _R_TH_CASE_AMB
    q_dissipated = (t_wind - _T_AMB) / r_th_total
    dt_wind = (p_copper - q_dissipated) / _C_TH_WIND

    return np.array([di_d, di_q, domega, dtheta, dt_wind])

def rhs_bldc_quad(t, y):
    """4-motor BLDC quad: 20-state DQ-frame PMSM with thermal coupling."""
    dy = np.zeros(20)
    for m in range(4):
        base = m * 5
        dy[base:base + 5] = _motor_rhs(y[base:base + 5], t, m)
    return dy