void main(void)
{
    //...
    for(int i = 0; i< 2; i++)
    {
        libmpu_init(&device, &device_configuration);
    }
    //...
}

void libmpu_init(libmpu_t *p_dev, libmpu_cfg_t *p_dev_cfg)
{
    assert(p_dev != NULL);
    assert(p_dev_cfg != NULL);

    unsigned char l_wake_up_device[2] = {LIBMPU_PWR_REG, LIBMPU_WAKEUP};

    p_dev->twi_cfg.clk_freq = TWI_CLK_FREQ_111KHz;
    p_dev->twi_cfg.bus = TWI_BUS_0;

    /* Initialize twi. */
    twi_init(&p_dev->twi, &p_dev->twi_cfg);

    /* Wake up device. */
    twi_start_communication(&p_dev->twi);
    twi_write(&p_dev->twi, l_wake_up_device, sizeof l_wake_up_device);
    twi_stop_communication(&p_dev->twi);

    _delay_ms(3000); // 3 seconds delay!
}

void twi_init(twi_t *p_this, twi_cfg_t *p_cfg)
{
    assert(p_this !=NULL);
    assert(p_cfg != NULL);

   /* Set pin as output. */
    DDRB = 0xFF;

    /* Set pin in tristate. */
    PORTB = 0x0;

    /* Bitrate */
    TWBR = 0x48;
    TWSR = 0;
}

void twi_start_communication(twi_t *p_this)
{
    assert(p_this != NULL);

    /* Generate start condition. */
    TWCR = (1<<(TWINT))|(1<<TWSTA )|(1<<TWEN) | (0<<TWSTO);
    while (!(TWCR &(1<<TWINT)));

    /* Initiate write.
     * http://www.ti.com/lit/an/slva704/slva704.pdf
     */
    twi_send_slave_address(LIBTWI_WRITE_OPERATION);
}

void twi_write(twi_t *p_this, char *p_buffer, int32_t p_buff_size)
{
    assert(p_this != NULL);

    for (int i = 0; i < p_buff_size; i++)
    {
        TWDR = p_buffer[i];
        TWCR = (1<<TWINT) | (1<<TWEN);

        while (!(TWCR & (1<<TWINT)));
    }
}

void twi_stop_communication(twi_t *p_this)
{
    assert(p_this != NULL);

    /* Send stop condition. */
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
    while (!(TWCR &(1<<TWINT)));
}