ok so there are 2 problems in the below code,
we went to testing today,
we found 2 problems
1) master is not going to the waypoints, but follower is moving
2) speed is not set to 3 meter per second, it is moving by default value

please tell why!

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, 10)

waypoints = [
    master_waypoint,
    (28.5861327, 77.3420592, 10),
    (28.5860912, 77.3420042, 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, 10)



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)
print("connection is done")

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


for drone in [master_drone, follower_drone]:
    drone.set_mode(4)
    drone.arm()
    drone.takeoff(10)
print("arming and takeoff is done")

# Initialize the previous_mode variable to None
previous_mode = {2: None, 3: None} # initialize the previous_mode dictionary

while True:
    msg = the_connection.recv_match(type='HEARTBEAT', blocking=False)
    if msg:
        sysid = msg.get_srcSystem()
        if sysid in [2, 3]:
            mode = mavutil.mode_string_v10(msg)
            if mode != previous_mode[sysid]: # check if the mode has changed
                previous_mode[sysid] = mode # update the previous_mode variable
                print(f"System ID: {sysid}, Mode: {mode}")
                
                # save the mode for sysid 2 and 3 in separate variables
                if sysid == 2:
                    mode_sysid_2 = mode
                elif sysid == 3:
                    mode_sysid_3 = mode

                    # Run the following code only when mode_sysid_3 is set to "GUIDED"
                    time_start = time.time()

                    if mode_sysid_3 == "GUIDED":
                        mode_sysid_2 = 4

                        while mode_sysid_2 == 4:
                            if abort():
                                exit()

                            # main loop
                            if time.time() - time_start >= 1:
                                for index, master_wp in enumerate(waypoints[:-1]):
                                    if mode_sysid_2 != 4:
                                        for drone in [master_drone, follower_drone]:
                                            drone.set_mode(6)
                                            drone.arm(False)
                                    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()
                                    if mode_sysid_2 != 4:
                                        for drone in [master_drone, follower_drone]:
                                            drone.set_mode(6)
                                            drone.arm(False)
                                    time.sleep(30)
                                # set the mode to rtl and disarms the drone
                                for drone in [master_drone, follower_drone]:
                                    drone.set_mode(6)
                                    drone.arm(False)
                                # set mode to rtl
                                master_drone.set_mode(6)
                                follower_drone.set_mode(6)
                                exit()
                            

the_connection.close()