python - AR.Drone2.0: No Definitive Movement When Executing Move Commands - Stack Overflow

admin管理员组

文章数量:1025251

I am currently working on my bachelor's thesis. As hardware programming is not the main focus of my studies, I might be missing a critical point, and I'm seeking the community's expertise to help resolve this issue.

I am using an AR.Drone2.0 and have written the following code to control its movements. However, despite issuing various movement commands, the drone does not exhibit any definitive movement. I suspect there might be an issue with the commands being sent or the timing between the main thread and the command thread.

I have tried taking off and landing, and these commands usually work. However, sometimes the script skips the landing command and exits without the drone actually landing, which is odd but not the main problem.

The drone tends to hover when I send the move command with the hover flag set to false. I expected the drone to be much more stable, but it moves around quite a lot.

The actual problem comes when I send a move command. In this case, the drone sometimes moves, but the movement seems undetermined and imprecise. For every unit of movement in the intended direction, it displays one unit of movement in two false directions, which I don't understand and haven't been able to solve on my own so far.

Your support is greatly appreciated.

Here is the relevant code:

class Drone:
    def __init__(self, ip: str, ports: dict) -> None:
        self.ip = ip  # IP address of the drone
        self_port = ports['com-port']
        self.nav_port = ports['nav-port']
        self.crit_port = ports['crit-port']
        
        # Setting up sockets
        self_socket = self.setup_socket()   #socket to send command
        self.nav_socket = self.setup_socket()   #socket to receive navdata 
 

        # Set up NavData listening in a separate thread
        self.nav_queue = queue.Queue()
        self.navdata_thread = threading.Thread(target=self.receive_navdata)
        self.navdata_thread.daemon = True  # Allow thread to exit when main program exits
        

        # Set up NavData processing in a separate thread
        self.decoded_nav_queue = queue.Queue()
        self.decode_navdata_thread = threading.Thread(target=self.decode_navdata)
        self.decode_navdata_thread.daemon = True  # Allow thread to exit when main program exits
        self.navdata_log: pd.DataFrame = pd.DataFrame()
        
        # Set up Commands in a separate thread
        selfmand_queue = queue.Queue()
        selfmand_counter = 1 
        selfmand_thread = threading.Thread(target=self.send_command)
        selfmand_thread.daemon = True  # Allow thread to exit when main program exits
        
        #start the threads
        self.decode_navdata_thread.start()
        self.navdata_thread.start() 
        selfmand_thread.start()

    def send_command(self):
       """Send control commands to the drone."""
       while True:
            if not selfmand_queue.empty():
                command = selfmand_queue.get()
                command = command.replace('#COMID#', str(selfmand_counter))
                selfmand_counter += 1
            else:
                command = 'AT*COMWDG\r'    
            self_socket.sendto(command.encode(), (self.ip, self_port))
            print()
            print()
            print(f"Sent control command: {command}")
            print()
            print()
            time.sleep(0.03)

    # Takeoff and landing is described in the documentation of the drone page 35.
    #  
    def takeoff(self) -> None:
        order = str(int('00010001010101000000001000000000',2))
        command = f'AT*REF=#COMID#,{order}\r'
        selfmand_queue.put(command)
        

    def land(self) -> None: 
        order = str(int('00010001010101000000000000000000',2))
        command = f'AT*REF=#COMID#,{order}\r'
        selfmand_queue.put(command)
        

    def move(self, hover: bool, lr_tilt: float, fb_tilt: float, vert_speed: float, yaw_speed: float) -> None:
        """
        Controls the drone's movement by adjusting its tilt and speed.

        Parameters:
        hover (bool): A flag indicating whether the drone should hover in place.
                      If True, the drone will maintain its current position and
                      orientation without any lateral, longitudinal, or vertical
                      movement. This is useful for stabilizing the drone in air.
                      If False, the drone will move according to the provided 
                      tilt and speed parameters.

        lr_tilt (float): The lateral or left-right tilt of the drone. 
                         Positive values tilt the drone to the right, 
                         while negative values tilt it to the left. 
                         Range: -1.0 to 1.0.

        fb_tilt (float): The longitudinal or forward-backward tilt of the drone.
                         Positive values tilt the drone forward, facilitating 
                         forward movement, whereas negative values tilt it backward.
                         Range: -1.0 to 1.0.

        vert_speed (float): The vertical speed of the drone, dictating its 
                            ascent or descent. Positive values increase the 
                            altitude, while negative values decrease it.
                            Range: -1.0 to 1.0.

        yaw_speed (float): The rotational speed of the drone around its vertical 
                           axis. Positive values rotate the drone clockwise, and 
                           negative values rotate it counterclockwise.
                           Range: -1.0 to 1.0.
        """
        if hover == True:
            flag = 0
        else:
            flag = 1

        lr_tilt = int(lr_tilt * 1000.0)
        fb_tilt = int(fb_tilt * 1000.0)
        vert_speed = int(vert_speed * 1000.0)
        yaw_speed = int(yaw_speed * 1000.0)
        command = f'AT*PCMD=#COMID#,{flag},{lr_tilt},{fb_tilt},{vert_speed},{yaw_speed}\r'
        selfmand_queue.put(command)
        

if __name__ == "__main__":
    ports = {'com-port': 5556, 'nav-port': 5554, 'crit-port': 5559}
    drone_ = Drone(ip="192.168.1.1", ports=ports)
    
    # Send the command to start NavData stream
    Drone.start_navdata_stream(self= drone_)
    
    #Example: Send a takeoff and land command
    drone_.takeoff()
    time.sleep(6)

    for _ in range(100):
        drone_.move(True, 0.0, 0.0, 0.0, 0.0)
    for _ in range(100):
        drone_.move(False, 0.0, -1.0, 0.0, 0.0)       
    time.sleep(6)
    drone_.land()
    time.sleep(3)

I am currently working on my bachelor's thesis. As hardware programming is not the main focus of my studies, I might be missing a critical point, and I'm seeking the community's expertise to help resolve this issue.

I am using an AR.Drone2.0 and have written the following code to control its movements. However, despite issuing various movement commands, the drone does not exhibit any definitive movement. I suspect there might be an issue with the commands being sent or the timing between the main thread and the command thread.

I have tried taking off and landing, and these commands usually work. However, sometimes the script skips the landing command and exits without the drone actually landing, which is odd but not the main problem.

The drone tends to hover when I send the move command with the hover flag set to false. I expected the drone to be much more stable, but it moves around quite a lot.

The actual problem comes when I send a move command. In this case, the drone sometimes moves, but the movement seems undetermined and imprecise. For every unit of movement in the intended direction, it displays one unit of movement in two false directions, which I don't understand and haven't been able to solve on my own so far.

Your support is greatly appreciated.

Here is the relevant code:

class Drone:
    def __init__(self, ip: str, ports: dict) -> None:
        self.ip = ip  # IP address of the drone
        self_port = ports['com-port']
        self.nav_port = ports['nav-port']
        self.crit_port = ports['crit-port']
        
        # Setting up sockets
        self_socket = self.setup_socket()   #socket to send command
        self.nav_socket = self.setup_socket()   #socket to receive navdata 
 

        # Set up NavData listening in a separate thread
        self.nav_queue = queue.Queue()
        self.navdata_thread = threading.Thread(target=self.receive_navdata)
        self.navdata_thread.daemon = True  # Allow thread to exit when main program exits
        

        # Set up NavData processing in a separate thread
        self.decoded_nav_queue = queue.Queue()
        self.decode_navdata_thread = threading.Thread(target=self.decode_navdata)
        self.decode_navdata_thread.daemon = True  # Allow thread to exit when main program exits
        self.navdata_log: pd.DataFrame = pd.DataFrame()
        
        # Set up Commands in a separate thread
        selfmand_queue = queue.Queue()
        selfmand_counter = 1 
        selfmand_thread = threading.Thread(target=self.send_command)
        selfmand_thread.daemon = True  # Allow thread to exit when main program exits
        
        #start the threads
        self.decode_navdata_thread.start()
        self.navdata_thread.start() 
        selfmand_thread.start()

    def send_command(self):
       """Send control commands to the drone."""
       while True:
            if not selfmand_queue.empty():
                command = selfmand_queue.get()
                command = command.replace('#COMID#', str(selfmand_counter))
                selfmand_counter += 1
            else:
                command = 'AT*COMWDG\r'    
            self_socket.sendto(command.encode(), (self.ip, self_port))
            print()
            print()
            print(f"Sent control command: {command}")
            print()
            print()
            time.sleep(0.03)

    # Takeoff and landing is described in the documentation of the drone page 35.
    #  
    def takeoff(self) -> None:
        order = str(int('00010001010101000000001000000000',2))
        command = f'AT*REF=#COMID#,{order}\r'
        selfmand_queue.put(command)
        

    def land(self) -> None: 
        order = str(int('00010001010101000000000000000000',2))
        command = f'AT*REF=#COMID#,{order}\r'
        selfmand_queue.put(command)
        

    def move(self, hover: bool, lr_tilt: float, fb_tilt: float, vert_speed: float, yaw_speed: float) -> None:
        """
        Controls the drone's movement by adjusting its tilt and speed.

        Parameters:
        hover (bool): A flag indicating whether the drone should hover in place.
                      If True, the drone will maintain its current position and
                      orientation without any lateral, longitudinal, or vertical
                      movement. This is useful for stabilizing the drone in air.
                      If False, the drone will move according to the provided 
                      tilt and speed parameters.

        lr_tilt (float): The lateral or left-right tilt of the drone. 
                         Positive values tilt the drone to the right, 
                         while negative values tilt it to the left. 
                         Range: -1.0 to 1.0.

        fb_tilt (float): The longitudinal or forward-backward tilt of the drone.
                         Positive values tilt the drone forward, facilitating 
                         forward movement, whereas negative values tilt it backward.
                         Range: -1.0 to 1.0.

        vert_speed (float): The vertical speed of the drone, dictating its 
                            ascent or descent. Positive values increase the 
                            altitude, while negative values decrease it.
                            Range: -1.0 to 1.0.

        yaw_speed (float): The rotational speed of the drone around its vertical 
                           axis. Positive values rotate the drone clockwise, and 
                           negative values rotate it counterclockwise.
                           Range: -1.0 to 1.0.
        """
        if hover == True:
            flag = 0
        else:
            flag = 1

        lr_tilt = int(lr_tilt * 1000.0)
        fb_tilt = int(fb_tilt * 1000.0)
        vert_speed = int(vert_speed * 1000.0)
        yaw_speed = int(yaw_speed * 1000.0)
        command = f'AT*PCMD=#COMID#,{flag},{lr_tilt},{fb_tilt},{vert_speed},{yaw_speed}\r'
        selfmand_queue.put(command)
        

if __name__ == "__main__":
    ports = {'com-port': 5556, 'nav-port': 5554, 'crit-port': 5559}
    drone_ = Drone(ip="192.168.1.1", ports=ports)
    
    # Send the command to start NavData stream
    Drone.start_navdata_stream(self= drone_)
    
    #Example: Send a takeoff and land command
    drone_.takeoff()
    time.sleep(6)

    for _ in range(100):
        drone_.move(True, 0.0, 0.0, 0.0, 0.0)
    for _ in range(100):
        drone_.move(False, 0.0, -1.0, 0.0, 0.0)       
    time.sleep(6)
    drone_.land()
    time.sleep(3)

本文标签： pythonARDrone20 No Definitive Movement When Executing Move CommandsStack Overflow