hi there,
make the below code more accurate and functionable and more complex
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):
        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],
            0, 0, 0, 0, 0, 0, 0
        ))

    def get_position(self):
        # Request the current state of the drone from the autopilot
        self.connection.mav.request_data_stream_send(self.system_id, self.connection.target_component,
                                                     mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1, 1)

        # Wait for a GLOBAL_POSITION_INT message and return the position as tuple (lat, lon, alt)
        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)

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

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

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_lat = PIDController(kp, ki, kd, limit=0.0001)
pid_lon = PIDController(kp, ki, kd, limit=0.0001)

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():
    """
    Waits for 7 seconds for the user to input ‘abort’. If ‘abort’ is entered,
    sets the mode to RTL and disarms the motors. Otherwise, continues with the waypoint.
    """
    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
    return False

mode = 4
print(f"{mode}")

time_start = time.time()
while mode == 4:
    # Abort drones if the abort command is given
    if abort():
        exit()

    if time.time() - time_start >= 1:  # Check if one second passed
        for master_wp in waypoints:
            master_drone.send_waypoint(master_wp)
            follower_position = master_drone.get_position()
            follower_wp = calculate_follower_coordinates(follower_position, distance, angle)

            # Call PID controller update for latitude and longitude
            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)

            # Adjust waypoint position using PID controller output
            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)
            time_start = time.time()
    time.sleep(0.1)  # Reduce sleep time to avoid too much delay

# Set mode to RTL and disarm both drones
for drone in [master_drone, follower_drone]:
    drone.set_mode(6)
    drone.arm(False)

the_connection.close()