Подключение акселерометра к микроконтроллеру и последующим снятием данных

void Orient_Init()
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
    Delay_ms(10);   
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); 
    
    GPIO_InitTypeDef pins;
    
    pins.GPIO_Pin = ORIENT_SDA_PIN | ORIENT_SCL_PIN;
    pins.GPIO_Speed = GPIO_Speed_2MHz;
    pins.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(ORIENT_I2C_PORT, &pins);  
    
    I2C_SoftwareResetCmd(ORIENT_I2C, ENABLE);
    Delay_ms(10);
    I2C_SoftwareResetCmd(ORIENT_I2C, DISABLE);   
    Delay_ms(10);
    
    I2C_InitTypeDef orient;
    
    orient.I2C_Mode = I2C_Mode_I2C;
    orient.I2C_ClockSpeed = 100000;
    orient.I2C_DutyCycle = I2C_DutyCycle_2;
    orient.I2C_OwnAddress1 = 0x01;
    orient.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    orient.I2C_Ack = I2C_Ack_Enable;
 
    I2C_Init(ORIENT_I2C, &orient);
    I2C_Cmd(ORIENT_I2C, ENABLE);     
    Delay_ms(100);
    LSM303_Init(LSM303_ADDR);
}

void Orient_StartTransmission(unsigned char address, int direction)
 {
    I2C_AcknowledgeConfig(ORIENT_I2C, ENABLE); 
    while(I2C_GetFlagStatus(ORIENT_I2C, I2C_FLAG_BUSY));
    I2C_GenerateSTART(ORIENT_I2C, ENABLE);
    while(!I2C_CheckEvent(ORIENT_I2C, I2C_EVENT_MASTER_MODE_SELECT));
    if(direction == I2C_Direction_Receiver)
    {
        I2C_Send7bitAddress(ORIENT_I2C, address << 1, I2C_Direction_Receiver);
        while(!I2C_CheckEvent(ORIENT_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
    }
    else
    {
        I2C_Send7bitAddress(ORIENT_I2C, address << 1, I2C_Direction_Transmitter);
        while(!I2C_CheckEvent(ORIENT_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
    }
 }
 
 void Orient_SendByte(unsigned char data)
 {
    I2C_SendData(ORIENT_I2C, data);
    while(!I2C_CheckEvent(ORIENT_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
 }
 
 unsigned char Orient_GetByte()
 {
     while(!I2C_CheckEvent(ORIENT_I2C, I2C_EVENT_MASTER_BYTE_RECEIVED));
     return I2C_ReceiveData(ORIENT_I2C);
 }
 
int LSM303_WriteByte(unsigned char HW_address, unsigned char address, unsigned char cmd)
{
     I2C_AcknowledgeConfig(ORIENT_I2C, ENABLE); 
     Orient_StartTransmission(HW_address, I2C_Direction_Transmitter);
     Orient_SendByte(address);
     Orient_SendByte(cmd);
     I2C_GenerateSTOP(ORIENT_I2C, ENABLE);
     return 1;
}    
unsigned char LSM303_ReadByte(unsigned char HW_address, unsigned char address)
{
    char data;
    I2C_AcknowledgeConfig(ORIENT_I2C, ENABLE);  
    Orient_StartTransmission(HW_address, I2C_Direction_Transmitter);
    I2C_GenerateSTOP(ORIENT_I2C, ENABLE);
    Orient_StartTransmission(HW_address, I2C_Direction_Receiver);    
    I2C_AcknowledgeConfig(ORIENT_I2C, DISABLE);
    data = Orient_GetByte();
    I2C_GenerateSTOP(ORIENT_I2C, ENABLE);
    return data;
}
int LSM303_ReadBurst(unsigned char HW_address, unsigned char address, int numbytes, Orient_Struct * Orient)
{
    I2C_AcknowledgeConfig(ORIENT_I2C, ENABLE); 
    Orient_StartTransmission(HW_address, I2C_Direction_Transmitter); 
    Orient_SendByte(address | (1 << 7));
    I2C_GenerateSTOP(ORIENT_I2C, ENABLE);
    Orient_StartTransmission(HW_address, I2C_Direction_Receiver); 
    for(int i = 0; i < numbytes - 1; i++)
        Orient->RawData[i] = Orient_GetByte();
    I2C_AcknowledgeConfig(ORIENT_I2C, DISABLE);  
    Orient->RawData[numbytes - 1] = Orient_GetByte();  
    I2C_GenerateSTOP(ORIENT_I2C, ENABLE);
    return 1;    
}

int LSM303_Init(unsigned char HW_address)
{
    unsigned char cmd = 0;
    if(!LSM303_WriteByte(HW_address, LSM303_INT_CTRL_M, cmd))
        return 0;    
    cmd = ((0x06 << 4) | 0x7);
    if(!LSM303_WriteByte(HW_address, LSM303_CTRL1, cmd))
        return 0;
    cmd = 0;
    cmd = (3 << 6);
    if(!LSM303_WriteByte(HW_address, LSM303_CTRL2, cmd))
        return 0;  
    cmd = 0;
    cmd = ((1 << 7) | (3 << 5) | (2 << 2));
    if(!LSM303_WriteByte(HW_address, LSM303_CTRL5, cmd))
        return 0;
    cmd = 0;
    if(!LSM303_WriteByte(HW_address, LSM303_CTRL6, cmd))
        return 0;   
    cmd = (1 << 6);
    if(!LSM303_WriteByte(HW_address, LSM303_CTRL7, cmd))
        return 0;   
    return 1;
}

int LSM303_GetA(unsigned char HW_address, Orient_Struct * Orient)
{
    Orient->Avalid = 0;
    if(LSM303_ReadBurst(HW_address, LSM303_OUT_X_L_A, 6, Orient) == 1)
    {
        Orient->RawAx = Orient->RawData[1] << 8 | Orient->RawData[0];
        Orient->RawAy = Orient->RawData[3] << 8 | Orient->RawData[2];
        Orient->RawAz = Orient->RawData[5] << 8 | Orient->RawData[4];        
        
        Orient->Ax = ((short)((Orient->RawData[1] << 8) | Orient->RawData[0]) * 0.000061) * 9.80065;
        Orient->Ay = ((short)((Orient->RawData[3] << 8) | Orient->RawData[2]) * 0.000061) * 9.80065;
        Orient->Az = ((short)((Orient->RawData[5] << 8) | Orient->RawData[4]) * 0.000061) * 9.80065;
        snprintf(Orient->formatA, 50, "Ax = %.4f \t Ay = %.4f \t Az = %.4f\r\n", Orient->Ax, Orient->Ay, Orient->Az);
        Orient->Avalid = 1;
        return Orient->Avalid;
    }
    else
        snprintf(Orient->formatA, 50, "Ax = -.---- \t Ay = -.---- \t Az = -.----\r\n");
        return 0;
}