c4dynamics.eqm.quadcopter.quadeqm

Contents

c4dynamics.eqm.quadcopter.quadeqm#

c4dynamics.eqm.quadcopter.quadeqm(t, X, quad, rotor_speeds, frame='NED')[source]#

Quadcopter equations of motion.

Computes the force and the moment of quadforces, adds gravity, and returns the state derivatives of eqm6 for a 12-state (euler angles) vector or of eqm6q for a 13-state (quaternion) vector. The signature fits scipy.integrate.solve_ivp:

sol = solve_ivp(c4d.eqm.quadeqm, [t, t + dt], quad.X, args = (quad, rotor_speeds, 'ENU'))
quad.X = sol.y[:, -1]
Parameters:

  • t (float) – Time [s]. Unused (the model is time invariant), kept for scipy.integrate.solve_ivp.

  • X (array_like) – State vector, 12 entries (euler angles) or 13 (quaternion). See quadforces.

  • quad (object) – The vehicle parameters as attributes: m mass [kg], g gravity [m/s^2], Ixx, Iyy, Izz principal moments of inertia [kg.m^2], and the parameters of quadforces.

  • rotor_speeds (array_like) – The four rotor speeds [rad/s], held constant over the call.

  • frame ({'NED', 'ENU'}, optional) – Inertial frame convention: 'NED' with a forward-right-down body frame (gravity along +z), 'ENU' with a forward-left-up body frame (gravity along -z). Defaults 'NED'.

Returns:

dX (numpy.ndarray) – State derivatives, the same length as X.

Examples

At hover, the thrust carries the weight and the vehicle stays put:

>>> import c4dynamics as c4d
>>> import numpy as np
>>> quad = c4d.struct(m=0.468, g=9.81, Ixx=4.856e-3, Iyy=4.856e-3, Izz=8.801e-3,
...                   kT=2.98e-6, kQ=1.14e-7, l=0.225, Ax=0.3, Ay=0.3, Az=0.25, Ar=0.2)
>>> w_hover = np.sqrt(quad.m * quad.g / 4 / quad.kT)
>>> dX = c4d.eqm.quadeqm(0, np.zeros(12), quad, [w_hover] * 4, 'ENU')
>>> np.allclose(dX, 0)
True

The same vehicle with a quaternion attitude:

>>> X = np.zeros(13)
>>> X[6] = 1   # identity quaternion
>>> np.allclose(c4d.eqm.quadeqm(0, X, quad, [w_hover] * 4, 'ENU'), 0)
True
Contents