RFM22B Init

    Spi_WriteData(SPI1, INTEN2, 0x00);      // Disable all interrupts
    Spi_WriteData(SPI1, OMFC1, 0x01);       // Set READY mode
    Spi_WriteData(SPI1, 0x09, 0x7F);        // Cap = 12.5pF
    Spi_WriteData(SPI1, 0x0A, 0x05);        // Clk output is 2MHz

    Spi_WriteData(SPI1, 0x0F, 0x70);        // NO ADC used
    Spi_WriteData(SPI1, 0x10, 0x00);        // no ADC used
    Spi_WriteData(SPI1, 0x12, 0x00);        // No temp sensor used
    Spi_WriteData(SPI1, 0x13, 0x00);        // no temp sensor used

    Spi_WriteData(SPI1, 0x70, 0x20);        // No manchester code, no data whiting, data rate < 30Kbps

    Spi_WriteData(SPI1, 0x1C, 0x1D);        // IF filter bandwidth
    Spi_WriteData(SPI1, 0x1D, 0x40);        // AFC Loop
    //write(0x1E, 0x0A);                    // AFC timing

    Spi_WriteData(SPI1, 0x20, 0xA1);        // clock recovery
    Spi_WriteData(SPI1, 0x21, 0x20);        // clock recovery
    Spi_WriteData(SPI1, 0x22, 0x4E);        // clock recovery
    Spi_WriteData(SPI1, 0x23, 0xA5);        // clock recovery
    Spi_WriteData(SPI1, 0x24, 0x00);        // clock recovery timing
    Spi_WriteData(SPI1, 0x25, 0x0A);        // clock recovery timing

    //write(0x2A, 0x18);
    Spi_WriteData(SPI1, 0x2C, 0x00);
    Spi_WriteData(SPI1, 0x2D, 0x00);
    Spi_WriteData(SPI1, 0x2E, 0x00);

    Spi_WriteData(SPI1, 0x6E, 0x27);        // TX data rate 1
    Spi_WriteData(SPI1, 0x6F, 0x52);        // TX data rate 0

    Spi_WriteData(SPI1, 0x30, 0x8C);        // Data access control

    Spi_WriteData(SPI1, 0x32, 0xFF);        // Header control

    Spi_WriteData(SPI1, 0x33, 0x42);        // Header 3, 2, 1, 0 used for head length, fixed packet length, synchronize word length 3, 2,

    Spi_WriteData(SPI1, 0x34, 64);          // 64 nibble = 32 byte preamble
    Spi_WriteData(SPI1, 0x35, 0x20);        // 0x35 need to detect 20bit preamble
    Spi_WriteData(SPI1, 0x36, 0x2D);        // synchronize word
    Spi_WriteData(SPI1, 0x37, 0xD4);
    Spi_WriteData(SPI1, 0x38, 0x00);
    Spi_WriteData(SPI1, 0x39, 0x00);
    Spi_WriteData(SPI1, 0x3A, 't');         // set tx header 3
    Spi_WriteData(SPI1, 0x3B, 'e');         // set tx header 2
    Spi_WriteData(SPI1, 0x3C, 's');         // set tx header 1
    Spi_WriteData(SPI1, 0x3D, 't');         // set tx header 0

    Spi_WriteData(SPI1, 0x3E, PACKET_LEN);  // set packet length

    Spi_WriteData(SPI1, 0x3F, 't');         // set rx header
    Spi_WriteData(SPI1, 0x40, 'e');
    Spi_WriteData(SPI1, 0x41, 's');
    Spi_WriteData(SPI1, 0x42, 't');
    Spi_WriteData(SPI1, 0x43, 0xFF);        // check all bits
    Spi_WriteData(SPI1, 0x44, 0xFF);        // Check all bits
    Spi_WriteData(SPI1, 0x45, 0xFF);        // check all bits
    Spi_WriteData(SPI1, 0x46, 0xFF);        // Check all bits

    Spi_WriteData(SPI1, 0x56, 0x01);

    Spi_WriteData(SPI1, 0x6D, 0x07);        // Tx power to +20dB

    Spi_WriteData(SPI1, 0x79, 0x00);        // no frequency hopping
    Spi_WriteData(SPI1, 0x7A, 0x00);        // no frequency hopping

    Spi_WriteData(SPI1, 0x71, 0x22);        // GFSK, fd[8]=0, no invert for TX/RX data, FIFO mode, txclk-->gpio

    Spi_WriteData(SPI1, 0x72, 0x48);        // Frequency deviation setting to 45K=72*625

    Spi_WriteData(SPI1, 0x73, 0x00);        // No frequency offset
    Spi_WriteData(SPI1, 0x74, 0x00);        // No frequency offset

    Spi_WriteData(SPI1, 0x75, 0x53);        // frequency set to 434MHz
    Spi_WriteData(SPI1, 0x76, 0x64);        // frequency set to 434MHz
    Spi_WriteData(SPI1, 0x77, 0x00);        // frequency set to 434Mhz

    Spi_WriteData(SPI1, 0x5A, 0x7F);
    Spi_WriteData(SPI1, 0x59, 0x40);
    Spi_WriteData(SPI1, 0x58, 0x80);

    Spi_WriteData(SPI1, 0x6A, 0x0B);
    Spi_WriteData(SPI1, 0x68, 0x04);
    Spi_WriteData(SPI1, 0x1F, 0x03);