Fw pos control: small mode cleanup

This small cleanup for the mode selection in the fw pos controller after #1476.

Also additional checks have been added to avoid that parts of the takeoff code run in non-auto modes.

This is HIL tested