Altitude Hold Control

Added the start of altitude hold control into autopilot mode 1. Function
elev() contains PI controller while motorUpdate() still executes
commands. Will evolve into a single position hold function with GPS.