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
3 changes: 3 additions & 0 deletions crazyflow/control/mellinger/control.py
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,8 @@ def state2attitude(
# maps thrust -> PWM -> rescaled thrust.
thrust = pwm2force(mass_thrust * current_thrust, thrust_max * 4, pwm_max)
command_rpyt = xp.concat((command_RPY, thrust[..., None]), axis=-1)
# l. 325 [controllerMellingerReset]
pos_err_i = xp.where((current_thrust > 0)[..., None], pos_err_i, 0.0)
return command_rpyt, pos_err_i


Expand Down Expand Up @@ -407,6 +409,7 @@ def _attitude2force_torque(
# l. 297 ff
torque_pwm = xp.clip(torque_pwm, -torque_pwm_max, torque_pwm_max)
torque_pwm = xp.where((force_des > 0)[..., None], torque_pwm, 0.0)
rot_err_i = xp.where((force_des > 0)[..., None], rot_err_i, 0.0)
force_des_pwm = force2pwm(force_des / 4, thrust_max, pwm_max)
pwms = force_torque_pwms2pwms(force_des_pwm, torque_pwm, mixing_matrix)
# We assume battery compensation is perfect. Motors below pwm_min are turned off. See
Expand Down
4 changes: 1 addition & 3 deletions tests/fuzzing/test_mellinger.py
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,6 @@ def test_params(drone: Drone):
np.testing.assert_allclose(state_params["mass"], ctrl.mass, rtol=1e-6)


@pytest.mark.xfail(strict=True, reason="TODO: Reset the integrators at zero thrust as the firmware")
@given(state=_states(), ctrl_state=_ctrl_states(), cmd=_state_cmds())
def test_state2attitude(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray):
"""Compare thrust, desired z axis and position integrators for state commands."""
Expand Down Expand Up @@ -172,7 +171,6 @@ def test_state2attitude(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, .
np.testing.assert_allclose(pos_err_i, [ref.i_error_x, ref.i_error_y, ref.i_error_z], atol=1e-6)


@pytest.mark.xfail(strict=True, reason="TODO: Reset the integrators at zero thrust as the firmware")
@given(state=_states(), ctrl_state=_ctrl_states(), cmd=_attitude_cmds())
def test_attitude2force_torque(
state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray
Expand Down Expand Up @@ -212,7 +210,7 @@ def test_attitude2force_torque(
@pytest.mark.xfail(
strict=True,
reason="TODO: Report the firmware levelling upside down in body rate mode once its z thrust "
"target turns negative, and reset the integrators at zero thrust as the firmware",
"target turns negative",
)
@given(state=_states(), ctrl_state=_ctrl_states(), cmd=_body_rate_cmds())
def test_body_rate2force_torque(
Expand Down
Loading