please give me the modifiesd code with pd controller 

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)

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

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)
            follower_drone.send_waypoint(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()