separate all telemetry code and add option to use softserial for telemetry. set softserial_baudrate=9600, softserial_inverted=1 and telemetry_softserial=1