def pid(self):
    # ... (previous PID code)

    # Calculate distance to the next waypoint
    distance_to_waypoint = math.sqrt(
        (self.drone_position[0] - self.setpoint[0]) ** 2 +
        (self.drone_position[1] - self.setpoint[1]) ** 2 +
        (self.drone_position[2] - self.setpoint[2]) ** 2
    )

    # Adjust speed based on distance to waypoint
    if distance_to_waypoint > 1.0:  # Example threshold for reducing speed (adjust as needed)
        self.speed_scaling_factor = 0.5  # Reduce speed when far from the waypoint
    else:
        self.speed_scaling_factor = 1.0  # Normal speed when close to the waypoint

    # Calculate PID control commands and scale them by the speed scaling factor
    self.cmd.rcRoll = int(1500 + self.error[0] * self.Kp[0] * self.speed_scaling_factor + ...)
    self.cmd.rcPitch = int(1500 + self.error[1] * self.Kp[1] * self.speed_scaling_factor + ...)
    self.cmd.rcThrottle = int(1554 + self.error[2] * self.Kp[2] * self.speed_scaling_factor + ...)

    # ... (rest of the PID code)












def pid(self):
    # ... (previous PID code)

    # Calculate distance to the next waypoint
    distance_to_waypoint = math.sqrt(
        (self.drone_position[0] - self.setpoints[self.current_setpoint_index][0]) ** 2 +
        (self.drone_position[1] - self.setpoints[self.current_setpoint_index][1]) ** 2 +
        (self.drone_position[2] - self.setpoints[self.current_setpoint_index][2]) ** 2
    )

    # Check if the drone is within the error range of ±0.2m for the current setpoint
    error_range = 0.2
    current_setpoint = self.setpoints[self.current_setpoint_index]
    if (
        abs(self.drone_position[0] - current_setpoint[0]) <= error_range and
        abs(self.drone_position[1] - current_setpoint[1]) <= error_range and
        abs(self.drone_position[2] - current_setpoint[2]) <= error_range
    ):
        # Drone is within the error range, move to the next setpoint
        self.current_setpoint_index += 1
        if self.current_setpoint_index < len(self.setpoints):
            # Update setpoint to the next waypoint
            self.setpoint = self.setpoints[self.current_setpoint_index]
            rospy.loginfo(f"Reached waypoint {self.current_setpoint_index}: {self.setpoint}")
        else:
            # All waypoints reached
            rospy.loginfo("All waypoints reached!")
            # Stop the drone or change state as needed

    # Adjust speed based on distance to waypoint
    if distance_to_waypoint > 1.0:  # Example threshold for reducing speed (adjust as needed)
        self.speed_scaling_factor = 0.5  # Reduce speed when far from the waypoint
    else:
        self.speed_scaling_factor = 1.0  # Normal speed when close to the waypoint

    # Calculate PID control commands and scale them by the speed scaling factor
    self.cmd.rcRoll = int(1500 + self.error[0] * self.Kp[0] * self.speed_scaling_factor + ...)
    self.cmd.rcPitch = int(1500 + self.error[1] * self.Kp[1] * self.speed_scaling_factor + ...)
    self.cmd.rcThrottle = int(1554 + self.error[2] * self.Kp[2] * self.speed_scaling_factor + ...)

    # ... (rest of the PID code)