status_t LSM303C::ACC_Status_Flags(uint8_t& val)
{
// printf("Getting accel status\n");
if( ACC_ReadReg(ACC_STATUS, val) )
{
printf("AERROR\n");
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_Status_Flags(uint8_t& val)
{
debug_println("Getting accel status");
if( ACC_ReadReg(ACC_STATUS, val) )
{
debug_println(AERROR);
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
float LSM303C::readAccelZ()
{
uint8_t flag_ACC_STATUS_FLAGS;
status_t response = ACC_Status_Flags(flag_ACC_STATUS_FLAGS);
if (response != IMU_SUCCESS)
{
printf("AERROR\n");
return NAN;
}
// Check for new data in the status flags with a mask
// If there isn't new data use the last data read.
// There are valid cases for this, like reading faster than refresh rate.
if (flag_ACC_STATUS_FLAGS & ACC_Z_NEW_DATA_AVAILABLE)
{
uint8_t valueL;
uint8_t valueH;
if ( ACC_ReadReg(ACC_OUT_Z_H, valueH) )
{
return IMU_HW_ERROR;
}
if ( ACC_ReadReg(ACC_OUT_Z_L, valueL) )
{
return IMU_HW_ERROR;
}
// printf("Fresh raw data\n");
//convert from LSB to mg
return(int16_t(( (valueH << 8) | valueL )) * SENSITIVITY_ACC);
}
// Should never get here
printf("Returning NAN\n");
return NAN;
}
status_t LSM303C::ACC_SetODR(ACC_ODR_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( ACC_ReadReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
value &= ~ACC_ODR_MASK;
value |= val;
if ( ACC_WriteReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_BlockDataUpdate(ACC_BDU_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( ACC_ReadReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
value &= ~ACC_BDU_ENABLE;
value |= val;
if ( ACC_WriteReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_EnableAxis(uint8_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( ACC_ReadReg(ACC_CTRL1, value) )
{
printf("AERROR\n");
return IMU_HW_ERROR;
}
value &= ~0x07;
value |= val;
if ( ACC_WriteReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_EnableAxis(uint8_t val)
{
debug_print(EMPTY);
uint8_t value;
if ( ACC_ReadReg(ACC_CTRL1, value) )
{
debug_println(AERROR);
return IMU_HW_ERROR;
}
value &= ~0x07;
value |= val;
if ( ACC_WriteReg(ACC_CTRL1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_SetFullScale(ACC_FS_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( ACC_ReadReg(ACC_CTRL4, value) )
{
printf("Failed ACC read\n");
return IMU_HW_ERROR;
}
value &= ~ACC_FS_8g;
value |= val;
if ( ACC_WriteReg(ACC_CTRL4, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::ACC_GetAccRaw(AxesRaw_t& buff)
{
uint8_t valueL;
uint8_t valueH;
if ( ACC_ReadReg(ACC_OUT_X_H, valueH) )
{
return IMU_HW_ERROR;
}
if ( ACC_ReadReg(ACC_OUT_X_L, valueL) )
{
return IMU_HW_ERROR;
}
buff.xAxis = (int16_t)( (valueH << 8) | valueL );
if ( ACC_ReadReg(ACC_OUT_Y_H, valueH) )
{
return IMU_HW_ERROR;
}
if ( ACC_ReadReg(ACC_OUT_Y_L, valueL) )
{
return IMU_HW_ERROR;
}
buff.yAxis = (int16_t)( (valueH << 8) | valueL );
if ( ACC_ReadReg(ACC_OUT_Z_H, valueH) )
{
return IMU_HW_ERROR;
}
if ( ACC_ReadReg(ACC_OUT_Z_L, valueL) )
{
return IMU_HW_ERROR;
}
buff.zAxis = (int16_t)( (valueH << 8) | valueL );
return IMU_SUCCESS;
}