Motor Model and Dynamics Simulation

Overview

Two modules form the physics layer of the simulator:

1. Motor model — maps desired [F, Mx, My, Mz] → desired motor RPMs → applies first-order lag → returns actual thrust and moments
2. Dynamics — nonlinear equations of motion; given forces and moments, returns the 16-element state derivative for ODE integration

Motor Model

Propeller Allocation Matrix (X-frame quadrotor)

The 4×4 allocation matrix maps [F, Mx, My, Mz] to squared motor RPMs. For an X-frame with arm length d, thrust coefficient cT, and torque coefficient cQ:

         motor:  1      2      3      4
Thrust:         +cT    +cT    +cT    +cT
Roll  (Mx):      0    +d·cT    0    -d·cT
Pitch (My):    -d·cT    0    +d·cT    0
Yaw   (Mz):    -cQ    +cQ    -cQ    +cQ

Implementation Logic

1. Build the 4×4 prop_matrix from cT, cQ, and d.
2. Solve prop_matrix @ rpm_sq = [F, Mx, My, Mz] for rpm_sq (desired squared RPMs).
3. Take sqrt of each element (clamp negatives to 0 first), then clip to [rpm_min, rpm_max].
4. Apply first-order motor lag: rpm_dot = km * (rpm_desired - rpm_current).
5. Compute actual force/moment using prop_matrix @ (motor_rpm ** 2) and return (F_actual, M_actual, rpm_dot).

Dynamics (Equations of Motion)

State vector (16,)

Indices	Meaning
0:3	Position [x, y, z]
3:6	Velocity [vẋ, vẏ, vż]
6:9	Euler angles [φ, θ, ψ]
9:12	Angular velocity [p, q, r]
12:16	Motor RPM [ω₁, ω₂, ω₃, ω₄]

Implementation Logic

Compute state_dot (16,) from current state and applied forces/moments:

1. Position derivative = current velocity (state[3:6]).
2. Velocity derivative = gravity + thrust projected into world frame via ZYX Euler rotation. The x/y accelerations depend on F/m, sin/cos of all three Euler angles. The z acceleration is −g + (F/m)·cos(φ)·cos(θ).
3. Euler angle derivative = current angular velocity (state[9:12]).
4. Angular velocity derivative = I⁻¹ M (solve inertia matrix against moment vector).
5. Motor RPM derivative = rpm_motor_dot from the motor model.

RK45 Integration

Use scipy.integrate.solve_ivp with method='RK45' over each timestep [t_k, t_{k+1}]. The ODE function wraps dynamics(params, s, F_actual, M_actual, rpm_motor_dot) with F_actual and M_actual held constant for the step. After solving, take the last column of sol.y as the new state.

Motor Physical Limits

Parameter	Value	Meaning
rpm_min	3000	Minimum motor speed (motors always spinning)
rpm_max	20000	Maximum motor speed
motor_constant km	36.5 s⁻¹	First-order time constant; τ = 1/km ≈ 27 ms

Max Thrust Calculation

Derived from motor limits:

• T_max = 4 * cT * rpm_max² (all 4 motors at max RPM)
• T_min = 4 * cT * rpm_min²
• Thrust-to-weight ratio: twr = T_max / (mass * gravity)
• Max upward acceleration: (T_max − mass·g) / mass
• Max downward acceleration: (mass·g − T_min) / mass