Back to d3 controller, added altitude hold