Fix MSP/Telemetry Arm/Disarm problems.

Tested with two MSP ports, one via softserial. Both with telemetry
enabled and without telemetry enabled.