mc_pos_control: Improve maximum height limiter

Implements a better method of determining when to switch from velocity to altitude control to keep height limit from being exceeded.
This method removes the overshoot and transients in height caused by the switching of the previous algorithm.