MSP430F5525: code isnt working w/i2c

Part Number: MSP430F5525

Hello,

I am using LSM6DSO32TR as accelerometer and i connected it to P3.0 and P3.1 pins in MSP430F5525.

I wrote a code but my code isnt working.

It doesnt pass write-level. I dont know does my sensor work or not. 

Please help me for this problem.

Thanks in advance.

My code:

#include <msp430.h>
#include <stdio.h>
#include <stdint.h>

#define USE_CLOCK
#define USE_I2C_USCI
#define LSM6DSO 0x6B


extern unsigned char *PTxData; // Pointer to TX data, defined in i2c.h
extern unsigned char TXByteCtr; // number of bytes to transmit, defined in i2c.h
int data[7];
int ACCEL_XFilter[4];
int ACCEL_YFilter[4];
int ACCEL_ZFilter[4];

void i2cinit(uint8_t saddr);
void i2c_write(unsigned char *DataBuffer, unsigned char ByteCtr);
void Write(uint8_t saddr, uint8_t sub, uint8_t data);
int Read(uint8_t saddr, uint8_t sub);
void Filters_Accel();

void main(void)
{
WDTCTL = WDTPW | WDTHOLD;

Write(0x6B, 0x10, 0x40);
Write(0x6B, 0x11, 0x40);
Write(0x6B, 0x15, 0x10);
Write(0x6B, 0x16, 0x80);

while(1){

data[0]=Read(LSM6DSO, 0x28);
data[1]=Read(LSM6DSO, 0x29);
data[2]=Read(LSM6DSO, 0x2A);
data[3]=Read(LSM6DSO, 0x2B);
data[4]=Read(LSM6DSO, 0x2C);
data[5]=Read(LSM6DSO, 0x2D);
data[6]=Read(LSM6DSO, 0x0F);

data[0]=(data[1]<<8)|data[0];
data[2]=(data[3]<<8)|data[2];
data[4]=(data[5]<<8)|data[4];
Filters_Accel();

}


}
void i2cinit(uint8_t saddr){
UCB0CTL1 |= UCSWRST;
UCB0CTL0 = UCMST+ UCMODE_3 + UCSYNC;
UCB0CTL1 |= UCSSEL_2;
UCB0BRW = 16;
UCB0I2CSA = saddr;
P3SEL |= BIT0 | BIT1;
UCB0CTL1 &= ~UCSWRST;
if(UCB0I2CSA==0x3C) UCB0IE |= UCTXIE;
else UCB0IE &= ~UCTXIE;
}

void Write(uint8_t saddr, uint8_t sub, uint8_t data){
i2cinit(saddr);

UCB0CTL1 |= UCTR + UCTXSTT;

while (!(UCB0IFG&UCTXIFG));
UCB0TXBUF = sub;
while (!(UCB0IFG&UCTXIFG));
UCB0TXBUF= data;
while (!(UCB0IFG&UCTXIFG));
UCB0CTL1 |= UCTXSTP;
UCB0IFG &= ~UCTXIFG;
while (UCB0CTL1 & UCTXSTP);
}


int Read(uint8_t saddr, uint8_t sub)
{
i2cinit(saddr);
int data;
while (UCB0CTL1 & UCTXSTP);
UCB0CTL1 |= UCTR + UCTXSTT;

while (!(UCB0IFG&UCTXIFG));
UCB0TXBUF = sub;
while (!(UCB0IFG&UCTXIFG));
UCB0CTL1 &= ~UCTR;
UCB0CTL1 |= UCTXSTT;
UCB0IFG &= ~UCTXIFG;

while (UCB0CTL1 & UCTXSTT);
data = UCB0RXBUF;
UCB0CTL1 |= UCTXSTP;
UCB0IFG &= ~UCTXIFG;
while (UCB0CTL1 & UCTXSTP);
return data;
}

void Filters_Accel()
{
ACCEL_XFilter[3] = ACCEL_XFilter[2];
ACCEL_XFilter[2] = ACCEL_XFilter[1];
ACCEL_XFilter[1] = ACCEL_XFilter[0];
ACCEL_XFilter[0] = data[0];
data[0] = (ACCEL_XFilter[3] + ACCEL_XFilter[2] + ACCEL_XFilter[1] + ACCEL_XFilter[0])/4;

ACCEL_YFilter[3] = ACCEL_YFilter[2];
ACCEL_YFilter[2] = ACCEL_YFilter[1];
ACCEL_YFilter[1] = ACCEL_YFilter[0];
ACCEL_YFilter[0] = data[2];
data[2] = (ACCEL_YFilter[3]+ACCEL_YFilter[2]+ACCEL_YFilter[1]+ACCEL_YFilter[0])/4;

ACCEL_ZFilter[3] = ACCEL_ZFilter[2];
ACCEL_ZFilter[2] = ACCEL_ZFilter[1];
ACCEL_ZFilter[1] = ACCEL_ZFilter[0];
ACCEL_ZFilter[0] = data[4];
data[4] = (ACCEL_ZFilter[3]+ACCEL_ZFilter[2]+ACCEL_ZFilter[1]+ACCEL_ZFilter[0])/4;
}