Add code for setting and holding altitude, Cleaned up code, Remove unneeded debug prints