From 5dd2405f370c96181325e8fe34f90e80f8140f93 Mon Sep 17 00:00:00 2001 From: Tomas Aas-Hansen Date: Mon, 15 Jun 2026 00:51:39 +0200 Subject: [PATCH] Fixed inverted controller mappings. --- data/config.json | 6 +- data/data.json | 26 ++++---- data/pid_configs.json | 128 +++++++++++++++++++++++++++++++++++++++ lib/controller.py | 25 ++++---- tests/test_controller.py | 18 +++--- 5 files changed, 168 insertions(+), 35 deletions(-) diff --git a/data/config.json b/data/config.json index b9cb54a..28b64b5 100644 --- a/data/config.json +++ b/data/config.json @@ -24,8 +24,8 @@ ] }, "pid_setpoint_rates": { - "roll": 20.0, - "pitch": 20.0, - "yaw": 90.0 + "roll": 45.0, + "pitch": 45.0, + "yaw": 60.0 } } \ No newline at end of file diff --git a/data/data.json b/data/data.json index 98b77d9..c3b2cd2 100644 --- a/data/data.json +++ b/data/data.json @@ -1,14 +1,14 @@ { "imu": { - "yaw": -67.08, - "pitch": -1.1, - "roll": 179.3, - "yr": 0.18, - "pr": 0.15, - "rr": 0.3, - "ax": 0.085, - "ay": -0.004, - "az": -0.042 + "yaw": -67.73, + "pitch": 9.46, + "roll": 161.53, + "yr": -0.13, + "pr": 0.37, + "rr": -0.11, + "ax": 0.11, + "ay": -0.009, + "az": 0.024 }, "9dof": { "acceleration": { @@ -74,14 +74,14 @@ "dptSet": 0.0 }, "resources": { - "sequence": 49, - "uptime_ms": 50908, - "cpu_percent": 5, + "sequence": 5873, + "uptime_ms": 5881564, + "cpu_percent": 4, "heap_used_percent": 2, "heap_free_kb": 501, "heap_total_kb": 512, "thread_count": 20, - "udp_rx_count": 900, + "udp_rx_count": 93052, "udp_rx_errors": 0 }, "control_telemetry": { diff --git a/data/pid_configs.json b/data/pid_configs.json index 5501599..7f3f399 100644 --- a/data/pid_configs.json +++ b/data/pid_configs.json @@ -94,5 +94,133 @@ "ki": 0.0, "kd": 0.0075 } + }, + "temp1": { + "surge": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "sway": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "heave": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "roll": { + "kp": 0.0175, + "ki": 0.0, + "kd": 0.0016 + }, + "pitch": { + "kp": 0.012, + "ki": 0.0, + "kd": 0.0 + }, + "yaw": { + "kp": 0.008, + "ki": 0.0, + "kd": 0.0075 + } + }, + "temp2": { + "surge": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "sway": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "heave": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "roll": { + "kp": 0.01775, + "ki": 0.0, + "kd": 0.002 + }, + "pitch": { + "kp": 0.015, + "ki": 0.0, + "kd": 0.0 + }, + "yaw": { + "kp": 0.008, + "ki": 0.0, + "kd": 0.0075 + } + }, + "temp3": { + "surge": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "sway": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "heave": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "roll": { + "kp": 0.01775, + "ki": 0.0, + "kd": 0.0025 + }, + "pitch": { + "kp": 0.015, + "ki": 0.0, + "kd": 0.00175 + }, + "yaw": { + "kp": 0.008, + "ki": 0.0, + "kd": 0.0075 + } + }, + "temp4": { + "surge": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "sway": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "heave": { + "kp": 0.0, + "ki": 0.0, + "kd": 0.0 + }, + "roll": { + "kp": 0.01775, + "ki": 0.0, + "kd": 0.0025 + }, + "pitch": { + "kp": 0.015, + "ki": 0.0, + "kd": 0.00175 + }, + "yaw": { + "kp": 0.008, + "ki": 0.0, + "kd": 0.0075 + } } } \ No newline at end of file diff --git a/lib/controller.py b/lib/controller.py index 1c1930e..3ef587e 100644 --- a/lib/controller.py +++ b/lib/controller.py @@ -686,24 +686,25 @@ def update(self): # --- BITMASK OUTPUT ---- # Read axes - heave = -self._read_axis(pygame.CONTROLLER_AXIS_RIGHTY, 3) # Right Y (inverted) - yaw = self._read_axis(pygame.CONTROLLER_AXIS_RIGHTX, 2) # Right X + right_x = self._read_axis(pygame.CONTROLLER_AXIS_RIGHTX, 2) + right_y = self._read_axis(pygame.CONTROLLER_AXIS_RIGHTY, 3) r2 = self._read_trigger(pygame.CONTROLLER_AXIS_TRIGGERRIGHT, 5) # R2 trigger l2 = self._read_trigger(pygame.CONTROLLER_AXIS_TRIGGERLEFT, 4) # L2 trigger trigger_delta = l2 - r2 if abs(trigger_delta) > self.DEADZONE: self.nudge_manipulator(trigger_delta, self.delay_ms / 1000) - # This runs while button 9 is held down L1 to make - # surge and sway controls toggleable to pitch and roll + left_shoulder = self._read_button(pygame.CONTROLLER_BUTTON_LEFTSHOULDER, 9) - if left_shoulder: # Pitch and roll control - pitch = -self._read_axis(pygame.CONTROLLER_AXIS_LEFTY, 1) # Left Y (inverted) - roll = self._read_axis(pygame.CONTROLLER_AXIS_LEFTX, 0) # Left X - surge = 0.0 - sway = 0.0 - else: # Surge and sway control - surge = -self._read_axis(pygame.CONTROLLER_AXIS_LEFTY, 1) # Left Y (inverted) - sway = self._read_axis(pygame.CONTROLLER_AXIS_LEFTX, 0) # Left X + surge = -self._read_axis(pygame.CONTROLLER_AXIS_LEFTY, 1) # Left Y (inverted) + sway = self._read_axis(pygame.CONTROLLER_AXIS_LEFTX, 0) # Left X + if left_shoulder: + heave = 0.0 + yaw = 0.0 + pitch = -right_y + roll = right_x + else: + heave = -right_y + yaw = -right_x pitch = 0.0 roll = 0.0 diff --git a/tests/test_controller.py b/tests/test_controller.py index 89b4cfc..399eb8d 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -138,7 +138,7 @@ def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypa assert command["surge"] == pytest.approx(0.5, rel=1e-3) assert command["sway"] == pytest.approx(0.25, rel=1e-3) assert command["heave"] == pytest.approx(0.125, rel=1e-3) - assert command["yaw"] == pytest.approx(0.375, rel=1e-3) + assert command["yaw"] == pytest.approx(-0.375, rel=1e-3) assert command["pitch"] == 0.0 assert command["roll"] == 0.0 assert command["manip"] == pytest.approx(-0.0144, rel=1e-3) @@ -150,12 +150,14 @@ def test_sdl_gamecontroller_mapping_normalizes_linux_playstation_layout(monkeypa assert status["buttons"][12] == 1.0 -def test_sdl_left_shoulder_switches_left_stick_to_pitch_and_roll(monkeypatch): +def test_sdl_left_shoulder_switches_right_stick_to_pitch_and_roll(monkeypatch): monkeypatch.setattr(pygame.event, "get", lambda: []) sdl = FakeSdlController( axes={ - pygame.CONTROLLER_AXIS_LEFTX: -32768, - pygame.CONTROLLER_AXIS_LEFTY: 32767, + pygame.CONTROLLER_AXIS_LEFTX: 8192, + pygame.CONTROLLER_AXIS_LEFTY: -16384, + pygame.CONTROLLER_AXIS_RIGHTX: -32768, + pygame.CONTROLLER_AXIS_RIGHTY: 32767, }, buttons={pygame.CONTROLLER_BUTTON_LEFTSHOULDER}, ) @@ -164,8 +166,10 @@ def test_sdl_left_shoulder_switches_left_stick_to_pitch_and_roll(monkeypatch): ctrl.update() command = ctrl.bm.calls[-1] - assert command["surge"] == 0.0 - assert command["sway"] == 0.0 + assert command["surge"] == pytest.approx(0.5, rel=1e-3) + assert command["sway"] == pytest.approx(0.25, rel=1e-3) + assert command["heave"] == 0.0 + assert command["yaw"] == 0.0 assert command["pitch"] == -1.0 assert command["roll"] == -1.0 status = ctrl.get_input_status() @@ -193,7 +197,7 @@ def test_raw_joystick_mapping_remains_available_for_unsupported_devices(monkeypa assert command["surge"] == 0.5 assert command["sway"] == 0.25 assert command["heave"] == 0.2 - assert command["yaw"] == 0.4 + assert command["yaw"] == -0.4 assert command["manip"] == pytest.approx(-0.0144, rel=1e-3) assert command["light"] == pytest.approx(0.1, rel=1e-3) status = ctrl.get_input_status()