revert previous altitude hold changes, old version was better
NOTE: compass way off when pitching forward/back
NOTE: GPS seems to work, but inaccurate due to compass pitch issue
NOTE: GPS adjustments should be made before integration against rc input