DSM Telemetry support

void CYRF_SetTxRxMode(enum TXRX_State mode)
{
    if ( !(CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80) && !(CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80)) {
        // TXGO and RXGO are not set, set end state.
        CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28); //force into Synth(TX) end state
        CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x08); //set Synth(TX) as end state
    }

void CYRF_SetTxRxMode(enum TXRX_State mode)
{
    //Set the post tx/rx state
    if((mode == TX_EN) && !(CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80)) 
        CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28);
    if((mode == RX_EN) && !(CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80) && !(CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80)) 
        CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28); 

void CYRF_SetTxRxMode(enum TXRX_State mode)
{
    //Set the post tx/rx state
    if((mode == TX_EN) && !(CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80)) 
        CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28);

#if HAS_MULTIMOD_SUPPORT

    txState++;
    if(txState == 16) { //2.3msec have passed
        if (CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80) { //We're still in receive mode
            //Disrupt any pending receive by enabling abort
            //Force End State should not be used to abort a receive if a SOP has already happened
            CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x20);
            //Buffer clean-up
            CYRF_RxPacketIsGood(0x00);
            //Abort by writing the FRC_END_STATE bit in the XACT_CFG register
            CYRF_SetTxRxMode(TX_EN); //Write mode
            //Disable abort
            CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x00);    
        } 
        else CYRF_SetTxRxMode(TX_EN); //Write mode

    } else if(state == DSM2_CH2_READ_A || state == DSM2_CH2_READ_B) {
        //Read telemetry if needed and parse if good
        if (CYRF_RxPacketIsGood(0x10)) {
           CYRF_ReadDataPacket(packet);
           parse_telemetry_packet();
        }
        if (state == DSM2_CH2_READ_A && num_channels < 8) {
            state = DSM2_CH2_READ_B;
            CYRF_StartReceive(); //Prepare to receive
            return 11000;
        }
        if (state == DSM2_CH2_READ_A)
            state = DSM2_CH1_WRITE_B;
        else
            state = DSM2_CH1_WRITE_A;
        if (CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80) { //We're still in receive mode
            //Disrupt any pending receive by enabling abort
            //Force End State should not be used to abort a receive if a SOP has already happened
            CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x20);
            //Buffer clean-up
            CYRF_RxPacketIsGood(0x00);
            //Abort by writing the FRC_END_STATE bit in the XACT_CFG register
            CYRF_SetTxRxMode(TX_EN); //Write mode
            //Disable abort
            CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x00);    
        } 
        else CYRF_SetTxRxMode(TX_EN); //Write mode
        set_sop_data_crc();
        return READ_DELAY;
    } 
    return 0;
}

        }
        txState = 0;
        while(CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80) {}
    }
    return delay;
}