Control

drone_controllers.mellinger.control

...

state2attitude(pos, quat, vel, ang_vel, cmd, ctrl_errors=None, ctrl_info=None, ctrl_freq=100, *, mass, kp, kd, ki, gravity_vec, mass_thrust, int_err_max, thrust_max, pwm_max)

Compute the positional part of the mellinger controller.

All controllers are implemented as pure functions. Therefore, integral errors have to be passed as an argument and returned as well.

Parameters:

Name	Type	Description	Default
pos	Array	Drone position with shape (..., 3).	required
quat	Array	Drone orientation as xyzw quaternion with shape (..., 4).	required
vel	Array	Drone velocity with shape (..., 3).	required
ang_vel	Array	Drone angular drone velocity in rad/s with shape (..., 3).	required
cmd	Array	Full state command in SI units and rad with shape (..., 13). The entries are [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate].	required
ctrl_errors	tuple[Array, ...] | None	Tuple of integral errors. For state2attitude, the tuple contains a single array (..., 3) for the position integral error or is None.	None
ctrl_info	tuple[Array, ...] | None	Tuple of arrays with additional data. Not used in state2attitude.	None
ctrl_freq	float	Control frequency in Hz	100
mass	float	Drone mass used for calculations in the controller in kg.	required
kp	Array	Proportional gain for the position controller with shape (3,).	required
kd	Array	Derivative gain for the position controller with shape (3,).	required
ki	Array	Integral gain for the position controller with shape (3,).	required
gravity_vec	Array	Gravity vector with shape (3,). We assume gravity to be in the negative z direction. E.g., [0, 0, -9.81].	required
mass_thrust	float	Conversion factor from thrust to PWM.	required
int_err_max	Array	Range of the integral error with shape (3,). i_range in the firmware.	required
thrust_max	float	Maximum thrust in N.	required
pwm_max	float	Maximum PWM value.	required

Returns:

Type	Description
Array	The RPY collective thrust command [rad, rad, rad, N], and the integral error of the position
Array	controller.

attitude2force_torque(pos, quat, vel, ang_vel, cmd, ctrl_errors=None, ctrl_info=None, ctrl_freq=500, *, kR, kw, ki_m, kd_omega, int_err_max, torque_pwm_max, thrust_max, pwm_min, pwm_max, L, KM, KF, mixing_matrix)

Compute the attitude to desired force-torque part of the Mellinger controller.

Note

We omit the axis flip in the firmware as it has only been introduced to make the controller compatible with the new frame of the Crazyflie 2.1.

Parameters:

Name	Type	Description	Default
pos	Array	Drone position with shape (..., 3).	required
quat	Array	Drone orientation as xyzw quaternion with shape (..., 4).	required
vel	Array	Drone velocity with shape (..., 3).	required
ang_vel	Array	Drone angular drone velocity in rad/s with shape (..., 3).	required
cmd	Array	Commanded attitude (roll, pitch, yaw) and total thrust [rad, rad, rad, N].	required
ctrl_errors	tuple[Array, ...] | None	Tuple of integral errors. For attitude2force_torque, the tuple contains a single array (..., 3) for the angular velocity integral error or is None.	None
ctrl_info	tuple[Array, ...] | None	Tuple of arrays with additional data. Not used in attitude2force_torque.	None
ctrl_freq	int	Control frequency in Hz	500
kR	Array	Proportional gain for the rotation error with shape (3,).	required
kw	Array	Proportional gain for the angular velocity error with shape (3,).	required
ki_m	Array	Integral gain for the rotation error with shape (3,).	required
kd_omega	Array	Derivative gain for the angular velocity error with shape (3,).	required
int_err_max	Array	Range of the integral error with shape (3,). i_range in the firmware.	required
torque_pwm_max	Array	Maximum torque in PWM.	required
thrust_max	float	Maximum thrust in N.	required
pwm_min	float	Minimum PWM value.	required
pwm_max	float	Maximum PWM value.	required
ang_vel_des		Desired angular velocity in rad/s.	required
prev_ang_vel		Previous angular velocity in rad/s.	required
prev_ang_vel_des		Previous angular velocity command in rad/s.	required
L	float	Distance from the center of the quadrotor to the center of the rotor in m.	required
KM	float	Torque constant (Nm/RPM).	required
KF	float	Force constant (N/RPM).	required
mixing_matrix	Array	Mixing matrix for the motor forces with shape (4, 3).	required

Returns:

Type	Description
tuple[Array, Array, Array]	4 Motor forces [N], i_error_m

force_torque_pwms2pwms(force_pwm, torque_pwm, mixing_matrix)

Convert desired collective thrust and torques to rotor speeds using legacy behavior.

force_torque2rotor_vel(force, torque, *, thrust_min, thrust_max, L, KM, KF, mixing_matrix)

Convert desired collective thrust and torques to rotor speeds.

The firmware calculates PWMs for each motor, compensates for the battery voltage, and then applies the modified PWMs to the motors. We assume perfect battery compensation here, skip the PWM interface except for clipping, and instead return desired motor forces.

Note

The equivalent function in the crazyflie firmware is power_distribution from power_distribution_quadrotor.c.

Warning

This function assumes an X rotor configuration.

Parameters:

Name	Type	Description	Default
force	Array	Desired thrust in SI units with shape (...,).	required
torque	Array	Desired torque in SI units with shape (..., 3).	required
thrust_min	float	Minimum thrust in N.	required
thrust_max	float	Maximum thrust in N.	required
L	float	Distance from the center of the quadrotor to the center of the rotor in m.	required
KM	float	Torque constant (Nm/RPM).	required
KF	float	Force constant (N/RPM).	required
mixing_matrix	Array	Mixing matrix for the motor forces with shape (4, 3).	required

Returns:

Type	Description
Array	The desired motor forces in SI units with shape (..., 4).