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

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')


class Drone:
    def __init__(self, sysid, master_address):
        self.sysid = sysid
        self.master_address = master_address
        self.connection = mavutil.mavlink_connection(self.master_address, baud=57600)
        self.master_position = [0, 0, 0]  # Starting position for the master drone

    def receive_messages(self):
        try:
            msg = self.connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
            if msg.get_srcSystem() == self.sysid:
                self.master_position = [msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3]
        except Exception as e:
            logging.error("Error receiving messages: {}".format(e))

    def send_position_target(self, x, y, z, vx, vy, vz):
        try:
            self.connection.mav.set_position_target_global_int(
                time.time(),
                self.sysid,
                mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1,
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                0b110111111000,
                int(x * 10 ** 7),
                int(y * 10 ** 7),
                z,
                vx,
                vy,
                vz,
                0, 0, 0, 0, 0
            )
        except Exception as e:
            logging.error("Error sending position target: {}".format(e))

    def close(self):
        self.connection.close()


def distance_between_points(p1, p2):
    return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2 + (p1[2] - p2[2]) ** 2)


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


master_drone = Drone(sysid=2, master_address='/dev/ttyUSB0')
follower_drone = Drone(sysid=3, master_address='/dev/ttyUSB0')

follower_distance = 5  # Distance in meters between master and follower
follower_angle = math.radians(60)  # Angle in radians between master and follower (60 degrees clockwise from master)

waypoints = [
    (-35.363261, 149.165230, 50),  # Waypoint 1
    (-35.363361, 149.165130, 50),  # Waypoint 2
    (-35.363461, 149.165030, 50)   # Waypoint 3
]

mode = "guided(4)"  # Set the mode to guided(4)

for i, waypoint in enumerate(waypoints):
    while True:
        # Receive messages and update the master position
        master_drone.receive_messages()

        # Calculate the target position for the follower drone
        follower_x = master_drone.master_position[0] + follower_distance * math.cos(follower_angle)
        follower_y = master_drone.master_position[1] + follower_distance * math.sin(follower_angle)
        follower_z = master_drone.master_position[2]

        # Send the target position to the master and follower drones
        master_drone.send_position_target(master_drone.master_position[0], master_drone.master_position[1], master_drone.master_position[2], 3, 0, 0)  # Constant speed of 3 m/s in the x direction
        follower_drone.send_position_target(follower_x, follower_y, follower_z, 3, 0, 0)  # Constant speed of 3 m/s in the x direction

        # Wait for 1 second before sending the next set of commands
        time.sleep(1)

        # Check if the user wants to abort the mission
        if mode == "guided(4)" and abort():
            break

        # Check if the follower drone has reached the current waypoint
        if distance_between_points([follower_x, follower_y, follower_z], waypoint) < 1:
            print("Reached waypoint {}.".format(i + 1))
            break

master_drone.close()
follower_drone.close()