From a048c5e1fafe185da20a964e1dd7212e18148934 Mon Sep 17 00:00:00 2001 From: Martin Schuck Date: Wed, 18 Nov 2026 17:20:00 +0100 Subject: [PATCH] Reset integrators on zero thrust --- crazyflow/control/mellinger/control.py | 3 +++ tests/fuzzing/test_mellinger.py | 4 +--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 72c1486e..07ff08bd 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -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 @@ -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 diff --git a/tests/fuzzing/test_mellinger.py b/tests/fuzzing/test_mellinger.py index ea50f819..741a0293 100644 --- a/tests/fuzzing/test_mellinger.py +++ b/tests/fuzzing/test_mellinger.py @@ -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.""" @@ -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 @@ -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(