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ż]