/// <summary>
/// Adds a point
/// </summary>
/// <param name="point"></param>
/// <param name="pathpoint"></param>
public void AddPointWithGimball(Point point, PathPoint pathpoint)
{
point.Id = waypoints.Count;
waypoints.Add(point);
if (waypoints.Count == 1)
{
pathpoint.FirstWaypoint();
gimbal_update_parameters.Add(pathpoint.Wp.gimbal_parameters);
}
}