About to attempt to add altitude control. This all needs cleaned up this week