mc_pos_control: fix altitude limitation

Before if you were above the maximum altitude you could not command to
go down anymore until the position controller had overshoot to under the
maximum altitude again.