Move altitude hold code to MAVProxy module, Fix various bugs, Cleaned up code