Merge branch 'bad-calculation-in-the-algorithm-route-control-component' into 'develop'

#178 - Bad calculation in the AlgorithmRouteControl Component (Waypoints)

See merge request !104

sim/src/components/Algorithm_Routecontrol/algorithm_RouteControl_implementation.cpp

 /********************************************************************************
  * Copyright (c) 2020-2021 ITK Engineering GmbH
- *
+ *               2023 Hexad GmbH
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
  * http://www.eclipse.org/legal/epl-2.0.
@@ -259,7 +259,7 @@ void Algorithm_Routecontrol_Implementation::ReadWayPointData()
         if (i<n-1)
         {
             openScenario::TrajectoryPoint pointNext = trajectory.points.at(i+1);
-            vel = std::sqrt(std::pow(pointNext.x-point.x,2) + std::pow(pointNext.y-point.y,2)) / mTimeStep; // uniform motion approximation
+            vel = std::sqrt(std::pow(pointNext.x-point.x,2) + std::pow(pointNext.y-point.y,2)) / (pointNext.time-point.time); // uniform motion approximation
         }
         waypoints->at(i).longVelocity = vel;
     }