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