-(void)WaypointMission:(CLLocation*)WPLoca
{
    float VirtualStickBearing = [self getHeadingForDirectionFromCoordinate:self.aircraftLocation.coordinate toCoordinate:WPLoca.coordinate];
    float VirtualStickDistance = [self.aircraftLocation distanceFromLocation:WPLoca];
    double VirtualStickAltitudeDifference = ABS(round(self.aircraftAltitude) - WPLoca.altitude);
    NSLog(@"Current distance: %f to coord: %f , %f", VirtualStickDistance, WPLoca.coordinate.latitude, WPLoca.coordinate.longitude);
    if (VirtualStickAltitudeDifference > 1)
    {
        self.mYaw = VirtualStickBearing;
    }
    else if (ABS(self.aircraftAttitude.yaw - VirtualStickBearing)>5)
    {
        self.mYaw = VirtualStickBearing;
    }
    else if (VirtualStickDistance > 60)
    {
        self.mYaw = VirtualStickBearing;
        self.mXVelocity = 4.0;
    }
    else if (VirtualStickDistance > 2 && VirtualStickDistance <= 60)
    {
        self.mYaw = VirtualStickBearing;
        self.mXVelocity = MIN(VirtualStickDistance/2, 4.0);
    }
    else{
        self.mYaw = VirtualStickBearing;
        self.mXVelocity = 0;
        self.index = self.index + 1;
    }
    [self updateVirtualStick];
}