Support for walkera telemetry.

CYRF_WriteRegister(CYRF_06_RX_CFG, 0x48);

{CYRF_06_RX_CFG, 0x48},

            } else {
                CYRF_SetTxRxMode(RX_EN); //Receive mode
                CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x80); //Prepare to receive (do not enable any IRQ)
                //delay = 900;
                //txState = 9;                
            }

            } else {
                CYRF_SetTxRxMode(RX_EN); //Receive mode
                CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x80); //Prepare to receive (do not enable any IRQ)
                delay = 800;
                txState = 8;                
            }

CYRF_WriteRegister(CYRF_06_RX_CFG, 0x48);

static u16 devo_telemetry_cb()
{
    int delay;
    
    if (txState == 0) {
        DEVO_BuildPacket();
        CYRF_WriteDataPacket(packet);
        txState = 1;
        return 900;
    }
    if (txState == 1) {
        int i = 0;
        u8 reg;
        while (! ((reg = CYRF_ReadRegister(0x04)) & 0x02)) {
            if (++i >= NUM_WAIT_LOOPS)
                break;
        }
        if (((reg & 0x22) == 0x20) || (CYRF_ReadRegister(0x02) & 0x80)) {
     	      CYRF_Reset();
            cyrf_init();
            cyrf_set_bound_sop_code();
            CYRF_ConfigRFChannel(*radio_ch_ptr);
            //printf(" Rst CYRF\n");
            delay = 1500;
            txState = 3;
        } else {
            if (state == DEVO_BOUND) {
                /* exit binding state */
                state = DEVO_BOUND_3;
                cyrf_set_bound_sop_code();
            }
            if(pkt_num == 0 || bind_counter > 0) {
                delay = 1500;
                txState = 3;
            } else {
                CYRF_SetTxRxMode(RX_EN); //Receive mode
                CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x80); //Prepare to receive (do not enable any IRQ)
                txState = 2;
                return 1200;
            }
        }
    }
    if(txState == 2) {  // this won't be true in emulator so we need to simulate it somehow
        //Read telemetry if needed
        if(CYRF_ReadDataPacketLen(packet) == 1) {
            parse_telemetry_packet(packet);
        }
#ifdef EMULATOR
        u8 telem_bit = rand32() % 7; // random number in [0, 7)
        packet[0] =  TELEMETRY_ENABLE + telem_bit; // allow emulator to simulate telemetry parsing to prevent future bugs in the telemetry monitor
        //printf("telem 1st packet: 0x%x\n", packet[0]);
        for(int i = 1; i < 13; i++)
            packet[i] = rand32() % 256;
        parse_telemetry_packet(packet);
        for(int i = 0; i < TELEM_UPDATE_SIZE; i++)
            Telemetry.updated[i] = 0xff;
#endif
        delay = 300;
        txState = 3;
    }
    if(txState == 3) {
        CYRF_SetTxRxMode(TX_EN); //Write mode
        if(pkt_num == 0) {
            //Keep tx power updated
            CYRF_WriteRegister(CYRF_03_TX_CFG, 0x08 | Model.tx_power);
            radio_ch_ptr = radio_ch_ptr == &radio_ch[2] ? radio_ch : radio_ch_ptr + 1;
            CYRF_ConfigRFChannel(*radio_ch_ptr);
        }
        txState = 0;
    }
    return delay;
}

int CYRF_ReadDataPacketLen(u8 dpbuffer[])
{
    u8 rx_state;
    u8 length;
    
    rx_state = CYRF_ReadRegister(CYRF_07_RX_IRQ_STATUS);
    if (rx_state & 0x02) {              // receive complete
        if (!(rx_state & 0x01)) {       // RXC=1, RXE=0 then 2nd check is required (debouncing)
            rx_state |= CYRF_ReadRegister(CYRF_07_RX_IRQ_STATUS);
        }
        length = CYRF_ReadRegister(CYRF_09_RX_COUNT);
        if ((rx_state & 0x05) || (CYRF_ReadRegister(CYRF_08_RX_STATUS) & 0x08)) {  //if any error or CRC error
            if( !(CYRF_ReadRegister(CYRF_06_RX_CFG) & 0x02)) { //if buffer overwrite disabled (RXOW bit = 0)
                // bad data, empty buffer
                CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80);  // need to set RXOW before data read
                while (length) {
                    CYRF_ReadRegister(CYRF_21_RX_BUFFER);
                    length--;
                }
            }
            return 2;  //bad data
        } else {
            // good data (complete with no errors)
            CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80);  // need to set RXOW before data read
            ReadRegisterMulti(CYRF_21_RX_BUFFER, dpbuffer, length);
            return 1;  //good data
        }
    }
    return 0;  //receive not complete
}

static u16 devo_telemetry_cb()
{
    int delay;
    
    if (txState == 0) {
        DEVO_BuildPacket();
        CYRF_WriteDataPacket(packet);
        txState = 1;
        return 900;
    }
    if (txState == 1) {
        int i = 0;
        u8 reg;
        while (! ((reg = CYRF_ReadRegister(0x04)) & 0x02)) {
            if (++i >= NUM_WAIT_LOOPS)
                break;
        }
        if (((reg & 0x22) == 0x20) || (CYRF_ReadRegister(0x02) & 0x80)) {
     	      CYRF_Reset();
            cyrf_init();
            cyrf_set_bound_sop_code();
            CYRF_ConfigRFChannel(*radio_ch_ptr);
            printf("Rst CYRF\n");
            delay = 1500;
            txState = 3;
        } else {
            if (state == DEVO_BOUND) {
                /* exit binding state */
                state = DEVO_BOUND_3;
                cyrf_set_bound_sop_code();
            }
            if(pkt_num == 0 || bind_counter > 0) {
                delay = 1500;
                txState = 3;
            } else {
                CYRF_SetTxRxMode(RX_EN); //Receive mode
                CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x80); //Prepare to receive (do not enable any IRQ)
                txState = 2;
                return 1180;
            }
        }
    }
    if(txState == 2) {  // this won't be true in emulator so we need to simulate it somehow
        //Read telemetry if needed
        int i = 0;
        u8 rx_status;
        while ( !((rx_status = CYRF_ReadRegister(0x07)) & 0x20)) {
            if (++i > NUM_WAIT_LOOPS)
                break;
        }
        if((rx_status & 0x03) == 0x02) { // RXC=1, RXE=0 then 2nd check is required (debouncing)
            rx_status |= CYRF_ReadRegister(0x07);
        }
        if ((rx_status & 0x27) == 0x22) { // good data (complete with no errors)
            CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80); //Prepare to read buffer
            CYRF_ReadDataPacket(packet);
            parse_telemetry_packet(packet);
            if(i) printf(" Delay: %d\n", i);
        }
#ifdef EMULATOR
        u8 telem_bit = rand32() % 7; // random number in [0, 7)
        packet[0] =  TELEMETRY_ENABLE + telem_bit; // allow emulator to simulate telemetry parsing to prevent future bugs in the telemetry monitor
        //printf("telem 1st packet: 0x%x\n", packet[0]);
        for(int i = 1; i < 13; i++)
            packet[i] = rand32() % 256;
        parse_telemetry_packet(packet);
        for(int i = 0; i < TELEM_UPDATE_SIZE; i++)
            Telemetry.updated[i] = 0xff;
#endif
        delay = 320;
        txState = 3;
    }
    if(txState == 3) {
        CYRF_SetTxRxMode(TX_EN); //Write mode
        if(pkt_num == 0) {
            //Keep tx power updated
            CYRF_WriteRegister(CYRF_03_TX_CFG, 0x08 | Model.tx_power);
            radio_ch_ptr = radio_ch_ptr == &radio_ch[2] ? radio_ch : radio_ch_ptr + 1;
            CYRF_ConfigRFChannel(*radio_ch_ptr);
        }
        txState = 0;
    }
    return delay;
}