fixed z axis in waypoint.cpp