Basic altitude hold with sonar working

PID coefficients still need to be tuned.