Dear Tomas,
Below is my source code.
void Initial_GSensor(void)
{
CS_B_PutVal(1);//int the CS to High
GSensor_Write_Addr_Cmd(XYZ_DATA_CFG, 0x12); // +/-8g range with ~0.976mg/LSB
// Set_Roll_Over();
// Set_Transient_Detection();
GSensor_Write_Addr_Cmd(CTRL_REG1, 0x39); // ODR = 1.56Hz, Normal mode, Active mode
GSensor_Write_Addr_Cmd(CTRL_REG2, 0x02); // Reset, High Resolution mode
// Get_GSensor_Calibration();
}
void Get_GSensor_XYZ(void)
{
GSensor_Read_Addr_Cmd(OUT_X_MSB, 7);
X_MSB = GSensor_Data[1];
X_LSB = GSensor_Data[2];
Y_MSB = GSensor_Data[3];
Y_LSB = GSensor_Data[4];
Z_MSB = GSensor_Data[5];
Z_LSB = GSensor_Data[6];
G_Sensor.Xout_14bit = ((X_MSB << 8) | (X_LSB))>>2;
G_Sensor.Yout_14bit = ((Y_MSB << 8) | (Y_LSB))>>2;
G_Sensor.Zout_14bit = ((Z_MSB << 8) | (Z_LSB))>>2;
if(G_Sensor.Xout_14bit >= TWO_COMPLEMENT){
X = ((X_MSB << 8) | (X_LSB))>>2; // flip all the 14 bits
X = (X ^ 0x3FFF) + 1;
G_Sensor.Xout_g = X * SENSITIVITY * (-1); // Compute X-axis output value in g's
}else{
G_Sensor.Xout_g = G_Sensor.Xout_14bit*SENSITIVITY; // Compute X-axis output value in g's
}
if(G_Sensor.Yout_14bit >= TWO_COMPLEMENT){
Y = ((Y_MSB << 8) | (Y_LSB))>>2; // flip all the 14 bits
Y = (Y ^ 0x3FFF) + 1;
G_Sensor.Yout_g = Y * SENSITIVITY * (-1); // Compute Y-axis output value in g's
}else{
G_Sensor.Yout_g = G_Sensor.Yout_14bit*SENSITIVITY; // Compute Y-axis output value in g's
}
if(G_Sensor.Zout_14bit >= TWO_COMPLEMENT){
Z = ((Z_MSB << 8) | (Z_LSB))>>2; // flip all the 14 bits
Z = (Z ^ 0x3FFF) + 1;
G_Sensor.Zout_g = Z * SENSITIVITY * (-1); // Compute Z-axis output value in g's
}else{
G_Sensor.Zout_g = G_Sensor.Zout_14bit*SENSITIVITY; // Compute Z-axis output value in g's
}
}
void Get_GSensor_Calibration(void)
{
GSensor_Write_Addr_Cmd(CTRL_REG1, 0x18); //Set device in 100 Hz ODR, Standby
GSensor_Read_Addr_Cmd(OUT_X_MSB, 7);
X_MSB = GSensor_Data[1];
X_LSB = GSensor_Data[2];
Y_MSB = GSensor_Data[3];
Y_LSB = GSensor_Data[4];
Z_MSB = GSensor_Data[5];
Z_LSB = GSensor_Data[6];
G_Sensor.Xout_14bit = ((X_MSB << 8) | (X_LSB))>>2;
G_Sensor.Yout_14bit = ((Y_MSB << 8) | (Y_LSB))>>2;
G_Sensor.Zout_14bit = ((Z_MSB << 8) | (Z_LSB))>>2;
X_offset = G_Sensor.Xout_14bit / 8 * (-1);
Y_offset = G_Sensor.Yout_14bit / 8 * (-1);
Z_offset = (G_Sensor.Zout_14bit - SENSITIVITY) / 8 * (-1);
GSensor_Write_Addr_Cmd(CTRL_REG1, 0x39); // ODR = 1.56Hz, Reduced noise, Active mode
}
void GSensor_Read_Addr_Cmd(char Addr,word len)
{
isoSPI1_TComData temp_char;
word i;
Delay_ms_GSensor(1);
CS_B_PutVal(0);//CS low
Delay_ms_GSensor(1);
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_SendChar( ((~0x80) & Addr) ); // R/W, ADDR[6], ADDR[5], ADDR[4], ADDR[3], ADDR[2], ADDR[1], ADDR[0]
TimeOut_CNT++;
}
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_SendChar(Addr); // ADDR[7], X, X, X, X, X, X, X
TimeOut_CNT++;
}
Delay_ms_GSensor(1);
for(i=0;i<len;i++)
{
isoSPI1_SendChar(0); // dummy
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_RecvChar(&temp_char); // DATA[7], DATA[6], DATA[5], DATA[4], DATA[3], DATA[2], DATA[1], DATA[0]
TimeOut_CNT++;
}
GSensor_Data[i] = temp_char;
}
Delay_ms_GSensor(1);
CS_B_PutVal(1);//CS High
Delay_ms_GSensor(1);
}
void GSensor_Write_Addr_Cmd(char Addr,char data)
{
Delay_ms_GSensor(1);
CS_B_PutVal(0);//CS low
Delay_ms_GSensor(1);
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_SendChar( ((0x80) | Addr) ); // R/W, ADDR[6], ADDR[5], ADDR[4], ADDR[3], ADDR[2], ADDR[1], ADDR[0]
TimeOut_CNT++;
}
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_SendChar(Addr); // ADDR[7], X, X, X, X, X, X, X
TimeOut_CNT++;
}
Check_Value = 1;
TimeOut_CNT = 0;
while( (Check_Value != 0) && (TimeOut_CNT < 3000)){
Check_Value = isoSPI1_SendChar(data); // DATA[7], DATA[6], DATA[5], DATA[4], DATA[3], DATA[2], DATA[1], DATA[0]
TimeOut_CNT++;
}
Delay_ms_GSensor(1);
CS_B_PutVal(1);//CS High
Delay_ms_GSensor(1);
}