Devlog #2

I added real orientation states (φ, θ, ψ) and made thrust act along a single body axis instead of the free-vector cheat I’d been using. This is the point where the sim actually starts being a quadrotor.

Building the rotation matrix
Before touching the dynamics at all, I built a rotation matrix that converts “straight up, from the drone’s own perspective” into “which way that is, in world coordinates.” A quadrotor only ever pushes along its own up-direction, so the only way it moves sideways is by tilting, which redirects that one push.

I built the matrix as three separate small rotations (yaw, roll, pitch) multiplied together, one for each axis, in a fixed order. I tested it with a small function, bodyZAxis(phi, theta, psi), against three hand-checks:

Level (phi=theta=psi=0) → straight up, unchanged.
Small roll → a small tilt in y, x untouched.
Small pitch → a small tilt in x, y untouched.

All three passed, which confirmed the rotation math was doing what it was supposed to before wiring it into anything else.

Wiring it into droneDynamics

The state vector went from 6 to 9: position, velocity, and now phi/theta/psi. The control loop has split into:
Outer loop (same PID idea as before) computes a desired force vector, F_des, from position/velocity error plus a gravity feedforward term.

That desired force gets converted into a desired tilt angle, using a small-angle approximation (the sideways component of a tilted thrust vector is roughly thrust * angle, solved backwards for angle given how much sideways force we want). Clamped to ±0.5 rad so a big position error can’t force a nonsensical tilt.

The actual attitude states lag toward that desired tilt with a simple first-order lag (same idea as the motor lag in my old Python project, just applied to orientation instead of thrust).

Actual thrust direction is computed from the real (possibly still-lagging) attitude, not the desired one.

Bugs, in the order I found them

1. Sign error on the gravity feedforward in F_des, which had gravity pulling the “cancel gravity” term the wrong way.

2. Sign error on the final acceleration line, where I used the wrong sign on gravity there too, which sent the drone climbing to hundreds of meters instead of hovering.

3. A big position error was asking for tilts of multiple radians (past 90°), which is physically meaningless and was part of what caused the runaway climb. Fixed with the clamp mentioned above.

Where it stands now
With the signs fixed and the tilt clamped, the drone tracks through the same waypoint list as before, but now via real (if simplified) attitude dynamics. I can see roll and pitch spike up during each leg of the trip and settle back down as it arrives at a waypoint, instead of the instant, direction-agnostic thrust from the old model.

Next up
This 9-state model is still leaning on a simplified attitude lag instead of real rotational physics, with no torques, no moment of inertia, no angular velocity states. The next real step is adding those (a 12-state model), replacing the lag with actual torque-driven rotation. After that, there is differential flatness and real trajectory generation, which is the actual point of this whole project