float LSM303C::readTempC()
{
uint8_t valueL;
uint8_t valueH;
float temperature;
// Make sure temperature sensor is enabled
if( MAG_TemperatureEN(MAG_TEMP_EN_ENABLE))
{
return NAN;
}
if( MAG_ReadReg(MAG_TEMP_OUT_L, valueL) )
{
return NAN;
}
if( MAG_ReadReg(MAG_TEMP_OUT_H, valueH) )
{
return NAN;
}
temperature = (float)( (valueH << 8) | valueL );
temperature /= 8; // 8 digits/˚C
temperature += 25;// Reads 0 @ 25˚C
return temperature;
}
status_t LSM303C::MAG_XYZ_AxDataAvailable(MAG_XYZDA_t& value)
{
if ( MAG_ReadReg(MAG_STATUS_REG, (uint8_t&)value) )
{
return IMU_HW_ERROR;
}
value = (MAG_XYZDA_t)((int8_t)value & (int8_t)MAG_XYZDA_YES);
return IMU_SUCCESS;
}
status_t LSM303C::MAG_TemperatureEN(MAG_TEMP_EN_t val){
uint8_t value;
if( MAG_ReadReg(MAG_CTRL_REG1, value) )
{
return IMU_HW_ERROR;
}
value &= ~MAG_TEMP_EN_ENABLE; //mask
value |= val;
if( MAG_WriteReg(MAG_CTRL_REG1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_Z_AxOperativeMode(MAG_OMZ_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG4, value) )
{
return IMU_HW_ERROR;
}
value &= ~MAG_OMZ_ULTRA_HIGH_PERFORMANCE; //mask
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG4, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_SetFullScale(MAG_FS_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG2, value) )
{
return IMU_HW_ERROR;
}
value &= ~MAG_FS_16_Ga; //mask
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG2, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_BlockDataUpdate(MAG_BDU_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG5, value) )
{
return IMU_HW_ERROR;
}
value &= ~MAG_BDU_ENABLE; //mask
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG5, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_XY_AxOperativeMode(MAG_OMXY_t val)
{
debug_print(EMPTY);
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG1, value) )
{
return IMU_HW_ERROR;
}
value &= ~MAG_OMXY_ULTRA_HIGH_PERFORMANCE; //mask
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG1, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_SetMode(MAG_MD_t val)
{
// printf("EMPTY\n");
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG3, value) )
{
printf("Failed to read MAG_CTRL_REG3. 'Read': 0x");
printf("0x%d\n",value);
return IMU_HW_ERROR;
}
value &= ~MAG_MD_POWER_DOWN_2;
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG3, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
// Methods required to get device up and running
status_t LSM303C::MAG_SetODR(MAG_DO_t val)
{
// printf("EMPTY\n");
uint8_t value;
if(MAG_ReadReg(MAG_CTRL_REG1, value))
{
printf("Failed Read from MAG_CTRL_REG1\n");
return IMU_HW_ERROR;
}
// Mask and only change DO0 bits (4:2) of MAG_CTRL_REG1
value &= ~MAG_DO_80_Hz;
value |= val;
if(MAG_WriteReg(MAG_CTRL_REG1, value))
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_SetMode(MAG_MD_t val)
{
debug_print(EMPTY);
uint8_t value;
if ( MAG_ReadReg(MAG_CTRL_REG3, value) )
{
debug_print("Failed to read MAG_CTRL_REG3. 'Read': 0x");
debug_printlns(value, HEX);
return IMU_HW_ERROR;
}
value &= ~MAG_MD_POWER_DOWN_2;
value |= val;
if ( MAG_WriteReg(MAG_CTRL_REG3, value) )
{
return IMU_HW_ERROR;
}
return IMU_SUCCESS;
}
status_t LSM303C::MAG_GetMagRaw(AxesRaw_t& buff)
{
// printf("EMPTY\n");
uint8_t valueL;
uint8_t valueH;
// printf("& was false\n");
if( MAG_ReadReg(MAG_OUTX_L, valueL) )
{
return IMU_HW_ERROR;
}
if( MAG_ReadReg(MAG_OUTX_H, valueH) )
{
return IMU_HW_ERROR;
}
buff.xAxis = (int16_t)( (valueH << 8) | valueL );
if( MAG_ReadReg(MAG_OUTY_L, valueL) )
{
return IMU_HW_ERROR;
}
if( MAG_ReadReg(MAG_OUTY_H, valueH) )
{
return IMU_HW_ERROR;
}
buff.yAxis = (int16_t)( (valueH << 8) | valueL );
if( MAG_ReadReg(MAG_OUTZ_L, valueL) )
{
return IMU_HW_ERROR;
}
if( MAG_ReadReg(MAG_OUTZ_H, valueH) )
{
return IMU_HW_ERROR;
}
buff.zAxis = (int16_t)( (valueH << 8) | valueL );
return IMU_SUCCESS;
}
