Fixed fdir1 calculation
Moved servo calculations from VehicleModel to Vehicle, removed VehicleModel