Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 8 additions & 5 deletions crazyflow/control/mellinger/control.py
Original file line number Diff line number Diff line change
Expand Up @@ -409,8 +409,11 @@ def _attitude2force_torque(
torque_pwm = xp.where((force_des > 0)[..., None], torque_pwm, 0.0)
force_des_pwm = force2pwm(force_des / 4, thrust_max, pwm_max)
pwms = force_torque_pwms2pwms(force_des_pwm, torque_pwm, mixing_matrix)
idle = xp.all(pwms == 0, axis=-1, keepdims=True)
pwms = xp.where(idle, 0.0, xp.clip(pwms, pwm_min, pwm_max))
# We assume battery compensation is perfect. Motors below pwm_min are turned off. See
# https://github.com/bitcraze/crazyflie-firmware/blob/d794c5ca081d19aa1e1bbcb6fd4a3711784fb8b1/src/drivers/src/motors.c#L199-L202
pwms = xp.where(pwms < pwm_min, 0.0, pwms)
excess = xp.clip(xp.max(pwms, axis=-1, keepdims=True) - pwm_max, min=0.0)
pwms = xp.clip(pwms - excess, min=0.0)

# Info: The Mellinger controller in the firmware ends here. However, we enforce a standardized
# interface in the simulation from states -> attitude -> force_torque. We therefore need this
Expand Down Expand Up @@ -481,9 +484,9 @@ def force_torque2rotor_vel(
torque_forces = (torque * xp.asarray([1 / L, 1 / L, 1 / thrust2torque])) @ mixing_matrix
motor_forces = (torque_forces + force) / 4
# Clip motor forces on the thrust instead of PWM level.
idle = xp.all(force == 0, axis=-1, keepdims=True)
motor_forces = xp.where(idle, 0.0, xp.clip(motor_forces, thrust_min, thrust_max))
# Assume perfect battery compensation and calculate the desired motor speeds directly
motor_forces = xp.where(motor_forces < thrust_min, 0.0, motor_forces)
excess = xp.clip(xp.max(motor_forces, axis=-1, keepdims=True) - thrust_max, min=0.0)
motor_forces = xp.clip(motor_forces - excess, min=0.0)
return motor_force2rotor_vel(motor_forces, rpm2thrust)


Expand Down
13 changes: 7 additions & 6 deletions tests/fuzzing/test_mellinger.py
Original file line number Diff line number Diff line change
Expand Up @@ -266,11 +266,7 @@ def test_mixer(thrust: NDArray, torque: NDArray):
np.testing.assert_allclose(motors, [m.m1, m.m2, m.m3, m.m4], atol=3)


@pytest.mark.xfail(
strict=True,
reason="TODO: Clip as the firmware, uniform above the maximum and at 0, and use the current "
"and previous body rate setpoint",
)
@pytest.mark.xfail(strict=True, reason="TODO: Use the current and previous body rate setpoint")
@given(state=_states(), ctrl_state=_ctrl_states(), cmd=_state_cmds())
def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray):
"""Compare the rotor velocities for state commands."""
Expand Down Expand Up @@ -305,7 +301,12 @@ def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...],
assume(abs(out.thrust) > 1)
uncapped, capped = cf.motors_thrust_uncapped_t(), cf.motors_thrust_pwm_t()
cf.powerDistribution(out, uncapped)
cf.powerDistributionCap(uncapped, capped) # We always assume perfect battery compensation
# cffirmware does not run the battery compensation. The battery compensation turns off motors
# below the minimum thrust. We assume perfect compensation, so we clip manually
m = uncapped.motors
pwms = [m.m1, m.m2, m.m3, m.m4]
m.m1, m.m2, m.m3, m.m4 = [0 if pwm < params["pwm_min"] else pwm for pwm in (pwms)]
cf.powerDistributionCap(uncapped, capped)
m = capped.motors
pwms = np.array([m.m1, m.m2, m.m3, m.m4])
motor_forces = pwm2force(pwms, params["thrust_max"], params["pwm_max"])
Expand Down
3 changes: 2 additions & 1 deletion tests/unit/control/test_mellinger.py
Original file line number Diff line number Diff line change
Expand Up @@ -351,7 +351,8 @@ def test_attitude2force_torque_batch_zero_thrust(drone: str):
def test_force_torque2rotor_vel_batch_zero_force(drone: str):
# Drones with zero desired force must not be clipped because other drones have non-zero force.
controller = parametrize(force_torque2rotor_vel, drone)
force = np.array([[0.0], [0.2]])
thrust_min = load_fn_params(force_torque2rotor_vel, drone)["thrust_min"]
force = np.array([[0.0], [8 * thrust_min]])
torque = np.zeros((2, 3))
rotor_vel_batch = controller(force, torque)
rotor_vel_single = controller(force[0], torque[0])
Expand Down
Loading