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;
}