Copter: RTL bug fix to initial step is always a climb and not a descent

Also bug fix to hold yaw on take-off