Added Altitude control & climb mode