Skip to content
TheChatArchiveThe Chat Archive
gpt-4-0314· openaiHugging Face Suzhen/CodeChat

the below code is not working from pymavlink import mavutil import math import time…

  1. User

    the below code is not working from pymavlink import mavutil import math import time

    class Drone:

    def __init__(self, system_id, connection):
        self.system_id = system_id
        self.connection = connection
    
    def set_mode(self, mode):
        self.connection.mav.set_mode_send(
            self.system_id,
            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
            mode
        )
    
    def arm(self, arm=True):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, int(arm), 0, 0, 0, 0, 0,
                                              0)
    
    def takeoff(self, altitude):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude)
    
    def send_waypoint(self, wp, next_wp, speed):
        vx, vy, vz = calculate_velocity_components(wp, next_wp, speed)
        self.connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message(
            10,
            self.system_id,
            self.connection.target_component,
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
            int(0b110111111000),
            int(wp[0] * 10 ** 7),
            int(wp[1] * 10 ** 7),
            wp[2],
            vx, 
            vy, 
            vz, 
            0, 
            0, 
            0, 
            0,
            0  # Set vx, vy, and vz from calculated components
        ))
    
    def get_position(self):
        self.connection.mav.request_data_stream_send(
            self.system_id, self.connection.target_component,
            mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1, 1)
    
        while True:
            msg = self.connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
            if msg.get_srcSystem() == self.system_id:
                return (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)
    
    def get_battery_status(self):
        msg = self.connection.recv_match(type='SYS_STATUS', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.battery_remain
    
    def get_rc_status(self):
        msg = self.connection.recv_match(type='RC_CHANNELS_RAW', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.chan8_raw
    
    def get_gps_status(self):
        msg = self.connection.recv_match(type='GPS_RAW_INT', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.satellites_visible, msg.eph
    

    class PIDController:

    def __init__(self, kp, ki, kd, limit):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.limit = limit
        self.prev_error = 0
        self.integral = 0
    
    def update(self, error, dt):
        derivative = (error - self.prev_error) / dt
        self.integral += error * dt
        self.integral = max(min(self.integral, self.limit), -self.limit)  # Clamp the integral term
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.prev_error = error
        return output
    

    distance = 5 # Distance in meters angle = 60 # Angle in degrees kp = 0.1 ki = 0.01 kd = 0.05 pid_limit = 0.0001 pid_lat = PIDController(kp, ki, kd, pid_limit) pid_lon = PIDController(kp, ki, kd, pid_limit)

    def calculate_follower_coordinates(wp, distance, angle): earth_radius = 6371000.0 # in meters latitude_change = (180 * distance * math.cos(math.radians(angle))) / (math.pi * earth_radius) longitude_change = (180 * distance * math.sin(math.radians(angle))) / ( math.pi * earth_radius * math.cos(math.radians(wp[0]))) new_latitude = wp[0] + latitude_change new_longitude = wp[1] + longitude_change return (new_latitude, new_longitude, wp[2])

    def calculate_velocity_components(current_wp, next_wp, speed): dx = next_wp[0] - current_wp[0] dy = next_wp[1] - current_wp[1] dz = next_wp[2] - current_wp[2] dx2 = dx ** 2 dy2 = dy ** 2 dz2 = dz ** 2 distance = math.sqrt(dx2 + dy2 + dz2) vx = (dx / distance) * speed vy = (dy / distance) * speed vz = (dz / distance) * speed return vx, vy, vz

    def check_safety(master_drone, follower_drone): # 1. Check battery levels master_battery = master_drone.get_battery_status() follower_battery = follower_drone.get_battery_status() if master_battery is not None and master_battery < 25 or follower_battery is not None and follower_battery < 25: print("Battery low!") return False

    # 2. Check RC fail
    master_rc_status = master_drone.get_rc_status()
    follower_rc_status = follower_drone.get_rc_status()
    if master_rc_status is not None and master_rc_status < 1000 or follower_rc_status is not None and follower_rc_status < 1000:
        print("RC fail!")
        return False
    
    # 3. Check GPS fail
    master_gps_status = master_drone.get_gps_status()
    follower_gps_status = follower_drone.get_gps_status()
    if master_gps_status is not None and master_gps_status[0] < 6 or  follower_gps_status is not None and follower_gps_status[0] < 6 or  master_gps_status is not None and master_gps_status[1] > 200 or follower_gps_status is not None and follower_gps_status[1] > 200:
        print("GPS fail!")
        return False
    
    return True
    

    waypoints = [ (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10) ]

    the_connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)

    master_drone = Drone(3, the_connection) follower_drone = Drone(2, the_connection)

    Set mode to Guided and arm both drones

    for drone in [master_drone, follower_drone]: drone.set_mode(4) drone.arm() drone.takeoff(10)

    def abort(): print("Type 'abort' to return to Launch and disarm motors.") start_time = time.monotonic() while time.monotonic() - start_time < 7: user_input = input("Time left: {} seconds \n".format(int(7 - (time.monotonic() - start_time)))) if user_input.lower() == "abort": print("Returning to Launch and disarming motors…") for drone in [master_drone, follower_drone]: drone.set_mode(6) # RTL mode drone.arm(False) # Disarm motors return True print("7 seconds have passed. Proceeding with waypoint task...") return False

    mode = the_connection.mode_mapping()[the_connection.flightmode] print(f"the drone is currectly at mode {mode}")

    time_start = time.time() while mode == 4: if abort(): exit() # Keep checking safety safeguards if not check_safety(master_drone, follower_drone): break

    if time.time() - time_start >= 1:
        for index, master_wp in enumerate(waypoints[:-1]):
            next_wp = waypoints[index + 1]  
            master_drone.send_waypoint(master_wp, next_wp, speed=3)
            follower_position = master_drone.get_position()
            if follower_position is None:
                break
            follower_wp = calculate_follower_coordinates(follower_position, distance, angle)
            dt = time.time() - time_start
            pid_lat_output = pid_lat.update(follower_wp[0] - follower_position[0], dt)
            pid_lon_output = pid_lon.update(follower_wp[1] - follower_position[1], dt)
            adjusted_follower_wp = (
                follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2])
            follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=3)
        else:
            mode_mapping = the_connection.mode_mapping()
            current_mode = the_connection.flightmode
            mode = mode_mapping[current_mode]
            time_start = time.time()
            time.sleep(0.1)
            continue
        break
    
    for drone in [master_drone, follower_drone]:
        drone.set_mode(6)
        drone.arm(False)
    

    the_connection.close()

  2. Assistant

    The code seems to be correct in terms of syntax and logic. However, there may be a few issues that you need to verify.

    1. Make sure that the drones you are using support the MAVLink protocol and are compatible with the pymavlink library.

    2. Double-check your MAVLink connection settings. The code provided is using the following settings: /dev/ttyUSB0 for the port and 57600 for the baudrate. Ensure these settings are correct for your hardware setup. Adjust them accordingly.

    3. Verify the ArduPilot flight modes corresponding to the number in the set_mode() function calls. In the provided code, the flight modes used are:

    • 4: Guided mode is set when drone.set_mode(4) is called.
    • 6: Return to Launch (RTL) mode is set when drone.set_mode(6) is called.

    Make sure these match the flight modes supported by your drone hardware.

    1. You can try printing more status messages throughout the code to figure out which part exactly is not working as expected. For example, print the obtained positions, waypoint calculations, PID control adjustments, etc., to track the code execution flow.

    2. Additionally, there might be dependency issues or bugs in the libraries being used. Ensure that you have installed the latest versions of the required packages.

    3. Lastly, consider testing your hardware components (such as GPS and RC) separately to eliminate the possibility of a hardware malfunction.

    Keep in mind that it's not always easy to troubleshoot code without additional context and information about the hardware setup. If none of the above suggestions help, consider providing more information about your hardware configuration, any specific error messages you've encountered, or clarifying which part of the code is not working as expected.

  3. User

    give me the code with these print the obtained positions, waypoint calculations, PID control adjustments, etc., to track the code execution flow.

  4. Assistant

    Here's the code with additional print statements for better tracking of the execution flow:

    # ...
    
    class Drone:
    
        # ...
    
        def send_waypoint(self, wp, next_wp, speed):
            # Print wp and next_wp
            print("Current waypoint: {} | Next waypoint: {}".format(wp, next_wp))
    
            vx, vy, vz = calculate_velocity_components(wp, next_wp, speed)
            
            # Print velocity components
            print("Velocity components: vx={}, vy={}, vz={}".format(vx, vy, vz))
    
            self.connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message(
                # ...
    
    def calculate_follower_coordinates(wp, distance, angle):
        # ...
        new_latitude = wp[0] + latitude_change
        new_longitude = wp[1] + longitude_change
        print("Calculated follower coordinates: lat={}, lon={}, alt={}".format(new_latitude, new_longitude, wp[2]))
        return (new_latitude, new_longitude, wp[2])
    
    # ...
    
    while mode == 4:
        if abort():
            exit()
    
        if not check_safety(master_drone, follower_drone):
            break
    
        if time.time() - time_start >= 1:
            for index, master_wp in enumerate(waypoints[:-1]):
                next_wp = waypoints[index + 1]  
                master_drone.send_waypoint(master_wp, next_wp, speed=3)
                follower_position = master_drone.get_position()
                
                # Print follower position
                print("Follower position: {}".format(follower_position))
                
                if follower_position is None:
                    break
                follower_wp = calculate_follower_coordinates(follower_position, distance, angle)
                dt = time.time() - time_start
                pid_lat_output = pid_lat.update(follower_wp[0] - follower_position[0], dt)
                pid_lon_output = pid_lon.update(follower_wp[1] - follower_position[1], dt)
    
                # Print PID output adjustments
                print("PID adjustments: lat={}, lon={}".format(pid_lat_output, pid_lon_output))
    
                adjusted_follower_wp = (
                    follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2])
    
                # Print adjusted follower waypoint
                print("Adjusted follower waypoint: {}".format(adjusted_follower_wp))
    
                follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=3)
            else:
                # ...
                time_start = time.time()
                time.sleep(0.1)
                continue
            break
    
    # ...
    

    Remember that adding many print statements could slow down the execution of the code, so use them for debugging purposes only, and consider removing them when the code works as expected.

  5. User

    is the below code correct? from pymavlink import mavutil import math import time

    the_connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)

    waypoints = [ (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10) ]

    distance = 5 # Distance in meters angle = 60 # Angle in degrees kp = 0.1 ki = 0.01 kd = 0.05 pid_limit = 0.0001

    class Drone:

    def __init__(self, system_id, connection):
        self.system_id = system_id
        self.connection = connection
    
    def set_mode(self, mode):
        self.connection.mav.set_mode_send(
            self.system_id,
            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
            mode
        )
    
    def arm(self, arm=True):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, int(arm), 0, 0, 0, 0, 0,
                                              0)
    
    def takeoff(self, altitude):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude)
    
    def send_waypoint(self, wp, next_wp, speed):
        # Print wp and next_wp
        print("Current waypoint: {} | Next waypoint: {}".format(wp, next_wp))
        vx, vy, vz = calculate_velocity_components(wp, next_wp, speed)
        # Print velocity components
        print("Velocity components: vx={}, vy={}, vz={}".format(vx, vy, vz))
        self.connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message(
            10,
            self.system_id,
            self.connection.target_component,
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
            int(0b110111111000),
            int(wp[0] * 10 ** 7),
            int(wp[1] * 10 ** 7),
            wp[2],
            vx, 
            vy, 
            vz, 
            0, 
            0, 
            0, 
            0,
            0  # Set vx, vy, and vz from calculated components
        ))
    
    def get_position(self):
        self.connection.mav.request_data_stream_send(
            self.system_id, self.connection.target_component,
            mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1, 1)
    
        while True:
            msg = self.connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
            if msg.get_srcSystem() == self.system_id:
                return (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)
    
    def get_battery_status(self):
        msg = self.connection.recv_match(type='SYS_STATUS', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.battery_remain
    
    def get_rc_status(self):
        msg = self.connection.recv_match(type='RC_CHANNELS_RAW', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.chan8_raw
    
    def get_gps_status(self):
        msg = self.connection.recv_match(type='GPS_RAW_INT', blocking=False)
        if msg and msg.get_srcSystem() == self.system_id:
            return msg.satellites_visible, msg.eph
    

    class PIDController:

    def __init__(self, kp, ki, kd, limit):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.limit = limit
        self.prev_error = 0
        self.integral = 0
    
    def update(self, error, dt):
        derivative = (error - self.prev_error) / dt
        self.integral += error * dt
        self.integral = max(min(self.integral, self.limit), -self.limit)  # Clamp the integral term
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.prev_error = error
        return output
    

    pid_lat = PIDController(kp, ki, kd, pid_limit) pid_lon = PIDController(kp, ki, kd, pid_limit)

    def calculate_follower_coordinates(wp, distance, angle): earth_radius = 6371000.0 # in meters latitude_change = (180 * distance * math.cos(math.radians(angle))) / (math.pi * earth_radius) longitude_change = (180 * distance * math.sin(math.radians(angle))) / ( math.pi * earth_radius * math.cos(math.radians(wp[0]))) new_latitude = wp[0] + latitude_change new_longitude = wp[1] + longitude_change print("Calculated follower coordinates: lat={}, lon={}, alt={}".format(new_latitude, new_longitude, wp[2])) return (new_latitude, new_longitude, wp[2])

    def calculate_velocity_components(current_wp, next_wp, speed): dx = next_wp[0] - current_wp[0] dy = next_wp[1] - current_wp[1] dz = next_wp[2] - current_wp[2] dx2 = dx ** 2 dy2 = dy ** 2 dz2 = dz ** 2 distance = math.sqrt(dx2 + dy2 + dz2) vx = (dx / distance) * speed vy = (dy / distance) * speed vz = (dz / distance) * speed return vx, vy, vz

    def check_safety(master_drone, follower_drone): # 1. Check battery levels master_battery = master_drone.get_battery_status() follower_battery = follower_drone.get_battery_status() if master_battery is not None and master_battery < 25 or follower_battery is not None and follower_battery < 25: print("Battery low!") return False

    # 2. Check RC fail
    master_rc_status = master_drone.get_rc_status()
    follower_rc_status = follower_drone.get_rc_status()
    if master_rc_status is not None and master_rc_status < 1000 or follower_rc_status is not None and follower_rc_status < 1000:
        print("RC fail!")
        return False
    
    # 3. Check GPS fail
    master_gps_status = master_drone.get_gps_status()
    follower_gps_status = follower_drone.get_gps_status()
    if master_gps_status is not None and master_gps_status[0] < 6 or  follower_gps_status is not None and follower_gps_status[0] < 6 or  master_gps_status is not None and master_gps_status[1] > 200 or follower_gps_status is not None and follower_gps_status[1] > 200:
        print("GPS fail!")
        return False
    
    return True
    

    master_drone = Drone(3, the_connection) follower_drone = Drone(2, the_connection)

    Set mode to Guided and arm both drones

    for drone in [master_drone, follower_drone]: drone.set_mode(4) drone.arm() drone.takeoff(10)

    def abort(): print("Type 'abort' to return to Launch and disarm motors.") start_time = time.monotonic() while time.monotonic() - start_time < 7: user_input = input("Time left: {} seconds \n".format(int(7 - (time.monotonic() - start_time)))) if user_input.lower() == "abort": print("Returning to Launch and disarming motors…") for drone in [master_drone, follower_drone]: drone.set_mode(6) # RTL mode drone.arm(False) # Disarm motors return True print("7 seconds have passed. Proceeding with waypoint task...") return False

    mode = the_connection.mode_mapping()[the_connection.flightmode] print(f"the drone is currectly at mode {mode}")

    time_start = time.time() while mode == 4: if abort(): exit() # Keep checking safety safeguards if not check_safety(master_drone, follower_drone): break

    if time.time() - time_start >= 1:
        for index, master_wp in enumerate(waypoints[:-1]):
            next_wp = waypoints[index + 1]  
            master_drone.send_waypoint(master_wp, next_wp, speed=3)
            follower_position = master_drone.get_position()
            
            # Print follower position
            print("Follower position: {}".format(follower_position))
            
            if follower_position is None:
                break
            follower_wp = calculate_follower_coordinates(follower_position, distance, angle)
            dt = time.time() - time_start
            pid_lat_output = pid_lat.update(follower_wp[0] - follower_position[0], dt)
            pid_lon_output = pid_lon.update(follower_wp[1] - follower_position[1], dt)
    
            # Print PID output adjustments
            print("PID adjustments: lat={}, lon={}".format(pid_lat_output, pid_lon_output))
    
            adjusted_follower_wp = (
                follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2])
    
            # Print adjusted follower waypoint
            print("Adjusted follower waypoint: {}".format(adjusted_follower_wp))
    
            follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=2)
        else:
            mode_mapping = the_connection.mode_mapping()
            current_mode = the_connection.flightmode
            mode = mode_mapping[current_mode]
            time_start = time.time()
            time.sleep(0.1)
            continue
        break
    
    for drone in [master_drone, follower_drone]:
        drone.set_mode(6)
        drone.arm(False)
    

    the_connection.close()

  6. Assistant

    Yes, the code you provided now includes the print statments I suggested previously:

    1. Printing current waypoint and the next waypoint
    2. Printing velocity components: vx, vy, vz
    3. Printing calculated follower coordinates
    4. Printing follower position
    5. Printing PID adjustments
    6. Printing adjusted follower waypoint

    These print statements will help you track the execution flow and understand how the drone is performing, making it easier to debug any issues during flight. Please note that having many print statements could potentially slow down the execution of the code, so they should be used carefully and only for debugging purposes.