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

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

  1. User

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

    the_connection = mavutil.mavlink_connection('COM14', baud=57600) the_connection.wait_heartbeat()

    msg = the_connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True) master_waypoint = (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)

    waypoints = [ master_waypoint, (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10), # Repeat the first waypoint to make the drone return to its starting point ] 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)
                # master_waypoint = (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)
                # # waypoints.insert(0, master_waypoint)
                # return master_waypoint
    

    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) master_drone = Drone(2, the_connection) follower_drone = Drone(3, the_connection)

    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 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

    Set mode to Guided and arm both drones

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

    to check the curent mode --------------------------------------

    Print the current flight mode

    mode = the_connection.mode_mapping()[the_connection.flightmode] print("Current mode:", mode)

    mode =4

    print(f"the drone is currectly at mode {mode}")

    time_start = time.time() while mode == 4: if abort(): exit()

    # main loop
    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:
                for drone in [master_drone, follower_drone]:
                    drone.set_mode(6)
                    drone.arm(False)      
                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)
            if abort():
                exit()
    	# mode = the_connection.mode_mapping()[the_connection.flightmode]
            time.sleep(8)
        break
    
    # set the mode to rtl and disarms the drone
    for drone in [master_drone, follower_drone]:
        drone.set_mode(6)
        drone.arm(False)
    

    the_connection.close()

  2. Assistant

    The code seems to be a script for controlling and following waypoints using two drones, receiving their positions, and PID controllers for performing a following behavior accurately. Here are a few suggestions and notes:

    1. Double-check the initial waypoints provided in the waypoints list. Make sure their latitude and longitude are correct.

    2. The script is using the COM14 serial port to communicate with the drone. Make sure that this port is correct and available on your system.

    3. Correct the indentation in the line:

    mode = the_connection.mode_mapping()[the_connection.flightmode]
    

    The line should be indented one level to the right to match the while mode == 4: loop.

    1. The script has repetitive code for switching to RTL mode and disarming the motors. You can create a function for this purpose:
    def return_to_launch_and_disarm(drones):
        for drone in drones:
            drone.set_mode(6)  # RTL mode
            drone.arm(False)  # Disarm motors
    
    # Replace repetitive code with this function call.
    
    1. Make sure you have the necessary libraries installed (pymavlink).

    2. Ensure that the kp, ki, kd, and pid_limit values are properly tuned for your specific application and drone types.

    3. Test the script in a safe environment to avoid potential damages or injuries.

    Otherwise, the script appears to be correct and functional.