At Elodin, we build flight software .. and a simulation platform to make flight software development less painful.
‍

Last week we developed a drone control system using our simulation platform. We aim to build a simulation of a simple quadcopter with a common cascaded PID control strategy. This will serve as a starting point for more sophisticated control strategies and more complex vehicles, such as a VTOL quadplane.

Before starting work on the control system, we first need to build a simulation of the quadcopter by modeling the vehicle dynamics. Elodin already has a mature 6DOF implementation, so this post will focus on simulating the quadcopter-specific dynamics on top of it. In future posts, we'll cover the control theory and FSW implementation.

SITL = Flight Dynamics Modeling + Characterization + Flight Software (running on either a real flight computer or a dev machine)

The four rotors provide a quadcopter's control authority, which generates both thrust and torque. Since there are 6 degrees of freedom and only 4 independent inputs, quadcopters are inherently underactuated and the control system must account for this.

But let's not get ahead of ourselves. First, we need to model the transfer function of the ESC + motor + propeller system. This is fundamental to understanding how the vehicle will respond to PWM control inputs. There's plenty of literature on this topic, but it's hard to apply much of it because cheap ESCs tend to be black boxes with datasheets that range from sparse to non-existent. So, we'll have to rely on using a thrust stand to experimentally determine the relationship between the PWM signal sent to the ESC and the resulting thrust/torque generated by the motor. Motor characterization is a critical part of simulation; we'll cover this in more detail in a future post.

For now, we'll assume that the relationship is quadratic with a constant coefficient of 0 and quadratic and linear coefficients that have to be experimentally determined. This aligns well with both theoretical models and empirical data. To simplify the scaling function, let's normalize the PWM signal input and thrust output to [0, 1]. Given an (x,y) of (1,1), ax^2 + bx = y results in a + b = 1. So, the normalized scaling function can be represented in terms of a single "motor thrust coefficient" a, where ax^2 + (1-a)x = y.

An Elodin system can be used to model this relationship.
‍

@el.map
def motor_thrust_response(time: el.Time, pwm: MotorPwm, prev_thrust: Thrust) -> Thrust:
    motor_time_constant = 0.1 # experimentally determined
    motor_thrust_coef = 0.65 # experimentally determined
    poly_coefs = jnp.array([motor_thrust_coef, 1 - motor_thrust_coef, 0.0])
    scale_thrust = partial(jnp.polyval, poly_coefs)
    thrust = scale_thrust(pwm / MAX_PWM_THROTTLE) * max_thrust
    alpha = dt / (dt + motor_time_constant)
    return prev_thrust + alpha * (thrust - prev_thrust)

In addition to the non-linear scaling function, you'll notice that we also added a first-order filter to model the motor's response time. This is a simple way to account for the fact that motors don't respond instantaneously to changes in PWM; they have a settling time. The time constant is the parameter that characterizes the response of a linear time-invariant (LTI) system to a step input. It represents the time it takes for the system's step response to reach 63.2% of its final value.

Now that we can translate PWM signals into thrust, we need to do the same for torque. Here, we'll make the simplifying assumption that the torque generated by the motor is proportional to the thrust. This is not true, but it's difficult to accurately measure torque using DIY thrust stands. We'll revisit this assumption in the future.

To find the torque-thrust ratio, we'll use the Motor velocity constant, which is the Kv rating of the motor. The electric motor torque formula is τ = Kt * I, where τ is the torque, Kt is the motor torque constant, and I is the current. This, combined with the assumtion that Kt = 1/Kv, gives us the relationship between current and torque. If we measure the current draw of the motor at a given thrust, we can then correlate thrust with torque.
‍

# Calculate torque-thrust ratio at optimal thrust efficiency
KT = 60.0 / (2 * jnp.pi * KV)  # Motor torque coefficient
opt_torque = KT * opt_current  # Torque at optimal thrust efficiency
torque_ratio = opt_torque / opt_thrust  # Torque-thrust ratio

@el.map
def motor_torque(thrust: Thrust) -> Torque:
    return thrust * torque_ratio

We now have a model for calculating the thrust and torque generated by each motor. However, we still need to account for the fact that the motors are not aligned with the vehicle's center of mass. Since we want a rigid body simulation, we need to calculate the net thrust and torque generated by all four motors on the body. Calculating the net thrust is simple: just sum the thrust of all four motors in the positive z direction (assuming z is up):
‍

body_thrust = el.SpatialForce.from_linear(jnp.array([0.0, 0.0, jnp.sum(motor_thrust)]))

Calculating the net torque is a bit more complicated as there are two components: the torque generated by the motors and the torque generated about the center of mass due to a difference in thrust. The torque generated by the motors translates directly into a torque about the body's center of mass. The only thing we need to do is account for the direction of spin of each motor. The torque generated by a motor is in the direction opposite to the direction of spin.

‍

yaw_torque_sum = jnp.sum(motor_torque * motor_spin_dir)
yaw_torque = el.SpatialForce.from_torque(jnp.array([0.0, 0.0, yaw_torque_sum]))

The torque generated from thrust differences is more complicated because it depends on the vehicle's geometry and the moment arms between the motors and the center of mass. The moment arms can be normalized to the vehicle's length and width to separate the vehicle configuration from it's size. The moment arms can be calculated from the motor angles in the X-Y plane:

‍

def motor_torque_axes(motor_angles: jax.Array) -> jax.Array:
    def motor_position(theta: float) -> jax.Array:
        return jnp.array([math.cos(theta), math.sin(theta), 0.0])

    thrust_dir = jnp.array([0.0, 0.0, 1.0])
    return jnp.array(
        [jnp.cross(motor_position(theta), thrust_dir) for theta in motor_angles]
    )

# Angle of each motor in the quadcopter frame (rad)
quad_motor_angles = math.pi * jnp.array([0.75, 0.25, -0.25, -0.75])
# The axis of rotation for the torque produced by each motor
motor_torque_axes = motor_torque_axes(quad_motor_angles)

Then, we can calculate torque by summing the cross products of the moment arms and the thrust generated by each motor:

‍

pitch_roll_torque = el.SpatialForce.from_torque(
    jnp.sum(motor_torque_axes * motor_thrust[:, None] * motor_dist[:, None], axis=0)
)

Note that `motor_dist` is the distance of each motor from the center of mass. Also the resulting torque from thrust differences can only be in the pitch and roll directions. Finally, we can sum all the forces and torques to get the net force and torque on the body:

‍

@el.map
def body_thrust(thrust: Thrust, torque: Torque) -> BodyThrust:
    yaw_torque_sum = jnp.sum(motor_torque * motor_spin_dir)
    body_thrust = el.SpatialForce.from_linear(jnp.array([0.0, 0.0, jnp.sum(motor_thrust)]))
    yaw_torque = el.SpatialForce.from_torque(jnp.array([0.0, 0.0, yaw_torque_sum]))
    # additional torque from differential thrust:
    pitch_roll_torque = el.SpatialForce.from_torque(
        jnp.sum(motor_torque_axes * motor_thrust[:, None] * motor_dist[:, None], axis=0)
    )
    return body_thrust + yaw_torque + pitch_roll_torque

We're now pretty much ready to start simulating the quadcopter. Let's throw in some simple gravity and drag systems to make the sim a bit more realistic.

‍

@el.map
def gravity(inertia: el.Inertia, f: el.Force) -> el.Force:
    return f + el.SpatialForce.from_linear(
        jnp.array([0.0, 0.0, -9.81]) * inertia.mass()
    )

@el.map
def drag(v: el.WorldVel) -> BodyDrag:
    rel_v = -v.linear()
    rel_v_norm = jnp.linalg.norm(rel_v)
    return 0.2 * 0.5 * rel_v * rel_v_norm

Oh, and we should also convert the body frame forces to the world frame as that's what the 6DOF integrator expects:

‍

@el.map
def apply_body_forces(thrust: BodyThrust, drag: BodyDrag, pos: el.WorldPos, f: el.Force) -> el.Force:
    return (
        f + el.SpatialForce.from_linear(drag) + el.SpatialForce(pos.angular() @ thrust)
    )

Now, we just have to plumb these systems together (which Elodin makes incredibly easy due to its ECS architecture), and add a GLB asset to visualize the quadcopter:

‍

world = el.World()
drone = world.spawn(
    [
        el.Body(
            world_pos=el.SpatialTransform.from_linear(jnp.array([0.0, 0.0, 2.0])),
            inertia=el.SpatialInertia(1.0, inertia=jnp.array([0.1, 0.1, 0.2])),
        ),
        world.glb(os.path.abspath("./examples/drone/edu-450-v2-drone.glb")),
        Drone(),
    ],
    name="Drone",
)

effectors = (
    gravity
    | drag
    | motor_thrust_response
    | motor_torque
    | body_thrust
    | apply_body_forces
)

sys = el.six_dof(
    dt, effectors, integrator=el.Integrator.SemiImplicit
)

That's it! Let's run the simulation with some test motor inputs:
‍

As you can see, it's pretty hard to control the quadcopter with just raw motor PWM values. In the next part, we'll model some sensors so that we can implement an autopilot to control the quadcopter. Stay tuned!

‍