Bugfix and correct switching of speed when engaging AP in TOGA