Altitude Hold Re-Inclusion

Added Altitude Hold again. Mode 1 has alt hold + yaw control and Mode 2
has just alt hold. Improved bug with initialization of alt hold with
iTermT. Also removed motorUpdate function to leave individual functions
to update motors.