Pymavlink with Xbox/Ps5 controller

Blue Robotics Software
1 
import time
from pymavlink import mavutil
import pygame
from pygame import joystick
import serial
import serial.tools.list_ports

# Initialize pygame and the joystick
pygame.init()
pygame.joystick.init()

# Wait for a joystick to be connected


joystick = pygame.joystick.Joystick(0)
joystick.init()

# Connect to the drone
def find_com_port():
    ports = serial.tools.list_ports.comports()
    for port in ports:
        try:
            connection = mavutil.mavlink_connection(port.device, baud=57600)
            connection.wait_heartbeat()
            print(f"Connected to {port.device}")
            return connection
        except Exception as e:
            print(f"Connection to {port.device} failed: {e}")
            continue

def arm_drone(connection):
    connection.mav.command_long_send(
        connection.target_system,
        connection.target_component,
        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
        0, 1, 0, 0, 0, 0, 0, 0
    )
    msg = connection.recv_match(type='COMMAND_ACK', blocking=True)
    return msg

master = find_com_port()
master.wait_heartbeat()
print(arm_drone(master))
print(master.target_system, master.target_component)
print("Drone connected")

def send_manual_control(x, y, z, r, buttons):
    master.mav.manual_control_send(
        master.target_system,
        x,
        y,
        z,
        r,
        buttons
    )
    msg = master.recv_match(type='COMMAND_ACK', blocking=True)
    return msg


def scale_joystick_value(value, in_min, in_max, out_min, out_max):
    # Scale joystick value from its range to desired range
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

try:
    while True:
        # Process pygame events
        pygame.event.pump()

        # Read joystick axes
        x_axis = joystick.get_axis(0)
        y_axis = joystick.get_axis(1)
        z_axis = joystick.get_axis(2)  # Throttle
        r_axis = joystick.get_axis(3)  # Yaw

        # Convert joystick values to MAVLink manual control values
        x = scale_joystick_value(x_axis, -1, 1, -1000, 1000)
        y = scale_joystick_value(y_axis, -1, 1, -1000, 1000)
        z = scale_joystick_value(z_axis, -1, 1, -1000, 1000)    # Throttle is usually 0 to 1000
        r = scale_joystick_value(r_axis, -1, 1, -1000, 1000)

        # Get button states
        buttons = 0
        for i in range(joystick.get_numbuttons()):
            if joystick.get_button(i):
                buttons |= (1 << i)

        # Send manual control command to the drone
        print(send_manual_control(x, y, z, r, buttons))

        # Delay to avoid spamming commands
        time.sleep(3)

except KeyboardInterrupt:
    print("Exiting...")

finally:
    pygame.quit()

What am I doing wrong?

2

Hi @radman, welcome to the forum :slight_smile:

This is very difficult to answer meaningfully without any context about what you’re trying to achieve and the way(s) in which it is not working as you expected.

As some examples:

  • Is your joystick connecting to your computer?
  • Is your code able to receive inputs from the joystick?
  • Are there any error messages when you run your code, that you’d like help with resolving?
  • Is there something you expect your code to do that is not happening the way you would like?

As you’ve specified in your comment, ArduSub (for legacy reasons) expects the throttle value to be between 0 and 1000 - I’d recommend changing the -1000 in your scaling function to a 0, unless you’re running a custom vehicle firmware that processes negative throttle inputs here correctly.

By the way, I’ve edited your post to have the code in a code block, which formats it and makes it easier to read, or copy for testing. You can read more about that in the Formatting a Post/Comment section of the “How to Use the Forums” post :slight_smile:

1 Like