TWI_Init();

Hardware_TWI_Enable();
if(!L3G4200D_Init((unsigned short*)&SensorsRawData[0u]))
G_SENSOR_MODE |= SENSOR_L3G4200D;

USART_puts(eUSARTF0, "L3G4200D test\r\n");

    USART_puts(eUSARTF0, "TWI_Baud ");outputHex(TWI_BAUDSETTING);USART_puts(eUSARTF0, "\r\n");
    uint8_t l3g4200address=L3G4200D_ADDRESS1; // gyro can be assigned to 2 addresses, we check the two here
    
    USART_puts(eUSARTF0, "Trying address ");
    outputHex(l3g4200address);
    USART_puts(eUSARTF0, "...\r\n");
    
    // Задаем адрес ячейки для чтения
      sendBuffer[0]=0x0F; // WHOAMI регистр
    
    TWI_MasterWriteRead(&twiMaster, l3g4200address, &sendBuffer[0], 1, 1);
    while (twiMaster.status != TWIM_STATUS_READY); // ждем
    
    if (twiMaster.readData[0x00]!=L3G4200D_WHOAMI)
    {
        USART_puts(eUSARTF0,"Not found...\r\n");
        l3g4200address=L3G4200D_ADDRESS2;
        USART_puts(eUSARTF0,"Trying address ");
        outputHex(l3g4200address);
        USART_puts(eUSARTF0, "...\r\n");
        sendBuffer[0]=0x0F; // WHOAMI регистр
        TWI_MasterWriteRead(&twiMaster, l3g4200address, &sendBuffer[0], 1, 1);
        while (twiMaster.status != TWIM_STATUS_READY); // ждем
        
        if (twiMaster.readData[0x00]!=L3G4200D_WHOAMI)
        {
            USART_puts(eUSARTF0,"Not found...\r\n");
            USART_puts(eUSARTF0,"Please check I2C bus connection, pull ups on SDA/SCL, power to the gyro, decoupling capacitors etc...\r\n");
            for(;;);
        }
      }
      USART_puts(eUSARTF0, "L3G4200D answered !\r\n");
      USART_puts(eUSARTF0,"Configuring gyro for basic measurements, no filtering, 500sps...\r\n");
      
      sendBuffer[0]=0x20;//Адрес старта
      sendBuffer[1]=0x8F;//значение
      TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDRESS2, sendBuffer, 2, 0);
      while (twiMaster.status != TWIM_STATUS_READY); // ждем
      sendBuffer[0]=0x24;//Адрес старта
      sendBuffer[1]=0x02;//значение
      TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDRESS2, sendBuffer, 2, 0);
      while (twiMaster.status != TWIM_STATUS_READY); // ждем
      //sendBuffer[0]=0x23;//Адрес старта
      //sendBuffer[1]=0x90;//значение
      //TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDRESS2, sendBuffer, 2, 0);
      //while (twiMaster.status != TWIM_STATUS_READY); // ждем
      
      // Проверка что записали в регистры
      USART_puts(eUSARTF0,"Reading writed registr\r\n");
      sendBuffer[0]=0x20;
      TWI_MasterWriteRead(&twiMaster, l3g4200address, &sendBuffer[0], 1, 1);
      while (twiMaster.status != TWIM_STATUS_READY); // ждем
      outputHex(twiMaster.readData[0x00]);USART_puts(eUSARTF0,"  ");
      //sendBuffer[0]=0x23;
      //TWI_MasterWriteRead(&twiMaster, l3g4200address, &sendBuffer[0], 1, 1);
      //while (twiMaster.status != TWIM_STATUS_READY); // ждем
      //outputHex(twiMaster.readData[0x00]);USART_puts(eUSARTF0,"  ");
      sendBuffer[0]=0x24;
      TWI_MasterWriteRead(&twiMaster, l3g4200address, &sendBuffer[0], 1, 1);
      while (twiMaster.status != TWIM_STATUS_READY); // ждем
      outputHex(twiMaster.readData[0x00]);USART_puts(eUSARTF0,"  ");
      USART_puts(eUSARTF0,"\r\n");
            
      USART_puts(eUSARTF0,"Reading x,y,z data\r\n");


    while (1)
    {
        // read 3 int16 which is 6 bytes.
        sendBuffer[0]=(0x80|0x28); //
        TWI_MasterWriteRead(&twiMaster, l3g4200address, sendBuffer, 1, 6);
        while (twiMaster.status != TWIM_STATUS_READY); // ждем
        gyroscope[0]=((twiMaster.readData[1]<<8) | twiMaster.readData[0])/20;
        gyroscope[1]=((twiMaster.readData[3]<<8) | twiMaster.readData[2])/20;
        gyroscope[2]=((twiMaster.readData[5]<<8) | twiMaster.readData[4])/20;
            
        USART_puts(eUSARTF0,"Angles x,y,z in °/s : ");
        float angle;
        int i=0;
        while(i<3)
        {
            angle=gyroscope[i]*0.0175f;
            dtostrf(angle,5,4, buffer);
            USART_puts(eUSARTF0,buffer);
            if (i!=2) USART_puts(eUSARTF0,", ");
            i++;
        }
        
        USART_puts(eUSARTF0,"    raw : ");    

        i=0;
        while(i<3)
        {
            itoa( gyroscope[i], buffer, 10);  // output 16 bit signed value to decimal.      
            USART_puts(eUSARTF0,buffer);
            if (i!=2) USART_puts(eUSARTF0,", ");
            i++;
        }
        USART_puts(eUSARTF0,"\r\n");

        _delay_ms(250); // wait 100ms so user can read something
        _delay_ms(250); // wait 100ms so user can read something
        _delay_ms(250); // wait 100ms so user can read something
        _delay_ms(250); // wait 100ms so user can read something
        
    }

gyroscope[2]=((twiMaster.readData[5]<<8) | twiMaster.readData[4])/20;