Need an advise to upgrade my code!!!

I have a simple code of waypoints, object with this code will go to waypoints positions and everything works fine. But i have a problem: object`s speed falls when its near the next waypoint.Thanks for any help! Here is my code :

 using UnityEngine;
 
 using System.Collections;
 
 public class Waypoints2 : MonoBehaviour 
 {
 
     public Vector3[] waypoints = new Vector3[1];
     public float waypointRadius = 1.5f;
     public float damping = 0.1f;
     public bool loop = false;
     public float speed = 2.0f;
     public bool faceHeading = true;
  
     private Vector3 currentHeading,targetHeading;
     private int targetwaypoint;
     private Transform xform;
     
     private bool useRigidbody;
     private Rigidbody rigidmember;
  
     void OnTriggerEnter(Collider collision)
     {
        Destroy(gameObject);
     }
  
  
     // Use this for initialization
     protected void Start ()
     {    
        xform = transform;
     
        currentHeading = xform.forward;
        if(waypoints.Length<=0)
        {
           Debug.Log("No waypoints on "+name);
           enabled = false;
        }
        targetwaypoint = 0;
        if(rigidbody!=null)
        {
           useRigidbody = true;
           rigidmember = rigidbody;
        }
        else
        {
           useRigidbody = false;
        }
     }
  
  
     // calculates a new heading
     protected void FixedUpdate ()
     {
        targetHeading = waypoints[targetwaypoint] - xform.position;
  
        currentHeading = Vector3.Lerp(currentHeading,targetHeading,damping*Time.deltaTime);
     }
  
     // moves us along current heading
      void Update()
     {
         if(Freeze.freezed == true && transform.position.x < 180 )
         {
             
         }
         
        if(useRigidbody)
           rigidmember.velocity = currentHeading * speed;
        else
           xform.position +=currentHeading * Time.deltaTime * speed;

        if(faceHeading)
           xform.LookAt(xform.position+currentHeading);
  
        if(Vector3.Distance(xform.position,waypoints[targetwaypoint])<=waypointRadius)
        {
           targetwaypoint++;
           if(targetwaypoint>=waypoints.Length)
           {
              targetwaypoint = 0;
              if(!loop)
                 enabled = false;
           }
        }
     }
     
     // draws red line from waypoint to waypoint
     public void OnDrawGizmos()
     {
        Gizmos.color = Color.red;
        if(waypoints==null)
           return;
        for(int i=0;i< waypoints.Length;i++)
        {
           Vector3 pos = waypoints[i];
           if(i>0)
           {
              Vector3 prev = waypoints[i-1];
              Gizmos.DrawLine(prev,pos);
           }
        }
     }
 }