correct thrust limiting during landing with margin, don't reset position setpoint when switching from takeoff to posctl and allow high enough z velocity for position lock

in mc auto: do not reset the position sp while near the waypoint, should make switching to manual pos control smoother