Fixed some bugs in the alt hold controller to do with re-zeroing velocity. Cleaned the code some too.