New altitude controller and move function
Previous altitude controller was removed
Merge pull request #3 from ncos/alt_cntrl_and_move

Alt cntrl and move