Manual way point need more tuning