File content as of revision 3:bd353b8184cc:

/***************************************************************************
  This is a library for the LSM303 Accelerometer and magnentometer/compass

  Designed specifically to work with the Adafruit LSM303DLHC Breakout

  These displays use I2C to communicate, 2 pins are required to interface.

  Adafruit invests time and resources providing this open source code,
  please support Adafruit andopen-source hardware by purchasing products
  from Adafruit!

  Written by Kevin Townsend for Adafruit Industries.  
  BSD license, all text above must be included in any redistribution
 ***************************************************************************/


#include "Adafruit_LSM303_U.h"
#include "Serial_base.h"

static float _lsm303Accel_MG_LSB     = 0.001F;   // 1, 2, 4 or 12 mg per lsb
static float _lsm303Mag_Gauss_LSB_XY = 1100.0F;  // Varies with gain
static float _lsm303Mag_Gauss_LSB_Z  = 980.0F;   // Varies with gain

Serial serial( USBTX, USBRX);

/***************************************************************************
 ACCELEROMETER
 ***************************************************************************/
/***************************************************************************
 PRIVATE FUNCTIONS
 ***************************************************************************/

/**************************************************************************/
/*!
    @brief  Abstract away platform differences in Arduino wire library
*/
/**************************************************************************/
void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
{
  byte data[2] = {reg, value};
  i2c->write(address,data,2);
}

/**************************************************************************/
/*!
    @brief  Abstract away platform differences in Arduino wire library
*/
/**************************************************************************/
byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
{
  byte value;

  i2c->writeByte(address, reg);
  i2c->read(address, &value, 1);
 
  return value;
}

/**************************************************************************/
/*!
    @brief  Reads the raw data from the sensor
*/
/**************************************************************************/
void Adafruit_LSM303_Accel_Unified::read()
{
  // Read the accelerometer
  // OUT_X_L_A: X-axis acceleration data. The value is expressed in 2’s complement
  i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
  
  byte data[6];
  i2c->read(LSM303_ADDRESS_ACCEL, data, 6);

  // Shift values to create properly formed integer (low byte first)
  _accelData.x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
  raw.x        = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
  
  _accelData.y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
  raw.y        = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
  
  _accelData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
  raw.z        = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
}

/***************************************************************************
 CONSTRUCTOR
 ***************************************************************************/
 
/**************************************************************************/
/*!
    @brief  Instantiates a new Adafruit_LSM303 class
*/
/**************************************************************************/
Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
  _sensorID = sensorID;
}

/***************************************************************************
 PUBLIC FUNCTIONS
 ***************************************************************************/
 
/**************************************************************************/
/*!
    @brief  Setups the HW
*/
/**************************************************************************/
bool Adafruit_LSM303_Accel_Unified::begin()
{
  // Enable I2C
  // Enable the accelerometer (200Hz)
  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x67);
  
  // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
  // if we are connected or not
  uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
  serial.printf("LSM303_REGISTER_ACCEL_CTRL_REG1_A %d \r\n", reg1_a);
  if (reg1_a != 0x67)
  {
    return false;
  }
  
  //400 Hz; normal power mode; X/Y/Z enabled
  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0b01110111);
  
  return true;
}

/**************************************************************************/
/*! 
    @brief  Gets the most recent sensor event
*/
/**************************************************************************/
void Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
  /* Clear the event */
  memset(event, 0, sizeof(sensors_event_t));
  
  /* Read new data */
  read();

  event->version   = sizeof(sensors_event_t);
  event->sensor_id = _sensorID;
  event->type      = SENSOR_TYPE_ACCELEROMETER;
  event->timestamp = millis();
  event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
  event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
  event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
}

/**************************************************************************/
/*! 
    @brief  Gets the sensor_t data
*/
/**************************************************************************/
void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
  /* Clear the sensor_t object */
  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */
  strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
  sensor->name[sizeof(sensor->name)- 1] = 0;
  sensor->version     = 1;
  sensor->sensor_id   = _sensorID;
  sensor->type        = SENSOR_TYPE_ACCELEROMETER;
  sensor->min_delay   = 0;
  sensor->max_value   = 0.0F; // TBD
  sensor->min_value   = 0.0F; // TBD
  sensor->resolution  = 0.0F; // TBD
}

/***************************************************************************
 MAGNETOMETER
 ***************************************************************************/
/***************************************************************************
 PRIVATE FUNCTIONS
 ***************************************************************************/

/**************************************************************************/
/*!
    @brief  Abstract away platform differences in Arduino wire library
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
{
  byte data[2] = {reg, value};
  i2c->write(address,data,2);
}

/**************************************************************************/
/*!
    @brief  Abstract away platform differences in Arduino wire library
*/
/**************************************************************************/
byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
{
  byte value;
  i2c->writeByte(address, reg);
  i2c->read(address, &value, 1);

  return value;
}

/**************************************************************************/
/*!
    @brief  Reads the raw data from the sensor
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::read()
{
  // Read the magnetometer
  i2c->writeByte(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M);
  
  byte data[6];
  i2c->read(LSM303_ADDRESS_MAG, data, 6);
   
  
  // Shift values to create properly formed integer (low byte first)
  // *0.5 da _magData.z in Sättigung geht
  _magData.x = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
  raw.x      = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
  
  _magData.z = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
  raw.z      = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
  
  
//  uint32_t magZOverflow = ((uint16_t)data[4] | ((uint16_t)data[5] << 8));
//  if( (magZOverflow > 30000) && (magZOverflow < 32767) )
//  {
//      _magData.z = (int16_t)33023;
//      raw.z      = (int16_t)33023;
//  }
//  else
//  {
//       _magData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
//       raw.z      = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
//  }
  
  _magData.y = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
  raw.y      = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
  
  //serial.printf("%i\r\n", magZOverflow );
    
  // ToDo: Calculate orientation
  _magData.orientation = 0.0;
}

/***************************************************************************
 CONSTRUCTOR
 ***************************************************************************/
 
/**************************************************************************/
/*!
    @brief  Instantiates a new Adafruit_LSM303 class
*/
/**************************************************************************/
Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
  _sensorID = sensorID;
  _autoRangeEnabled = false;
}

/***************************************************************************
 PUBLIC FUNCTIONS
 ***************************************************************************/
 
/**************************************************************************/
/*!
    @brief  Setups the HW
*/
/**************************************************************************/
bool Adafruit_LSM303_Mag_Unified::begin()
{
  
  // Enable I2C
 
  
  // Enable the magnetometer
  write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);

  // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
  write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0x10);  
  uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
  serial.printf("LSM303_REGISTER_MAG_CRA_REG_M %d \r\n", reg1_a);
  if (reg1_a != 0x10)
  {
    return false;
  }
  

  
  //Temp measurement off; 220 Hz data outputrate
  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0b00011100);
  // Set the gain to +/- 1.9 Gauss;
  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, 0b01000000);
    _magGain = LSM303_MAGGAIN_1_9;
  //Continuous-conversion mode
  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M,  0b00000000);
  

  
  
  
  uint8_t reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
  serial.printf("CRA_REG_M register %u \r\n", reg);

  reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M);
  serial.printf("CRB_REG_M register %u \r\n", reg);
  
  reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M);
  serial.printf("MR_REG_M register %u \r\n", reg);

  return true;
}

/**************************************************************************/
/*! 
    @brief  Enables or disables auto-ranging
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
{
  _autoRangeEnabled = enabled;
}

/**************************************************************************/
/*!
    @brief  Sets the magnetometer's gain
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
{
  write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
  
  _magGain = gain;
 
  switch(gain)
  {
    case LSM303_MAGGAIN_1_3:
      _lsm303Mag_Gauss_LSB_XY = 1100;
      _lsm303Mag_Gauss_LSB_Z  = 980;
      break;
    case LSM303_MAGGAIN_1_9:
      _lsm303Mag_Gauss_LSB_XY = 855;
      _lsm303Mag_Gauss_LSB_Z  = 760;
      break;
    case LSM303_MAGGAIN_2_5:
      _lsm303Mag_Gauss_LSB_XY = 670;
      _lsm303Mag_Gauss_LSB_Z  = 600;
      break;
    case LSM303_MAGGAIN_4_0:
      _lsm303Mag_Gauss_LSB_XY = 450;
      _lsm303Mag_Gauss_LSB_Z  = 400;
      break;
    case LSM303_MAGGAIN_4_7:
      _lsm303Mag_Gauss_LSB_XY = 400;
      _lsm303Mag_Gauss_LSB_Z  = 355;
      break;
    case LSM303_MAGGAIN_5_6:
      _lsm303Mag_Gauss_LSB_XY = 330;
      _lsm303Mag_Gauss_LSB_Z  = 295;
      break;
    case LSM303_MAGGAIN_8_1:
      _lsm303Mag_Gauss_LSB_XY = 230;
      _lsm303Mag_Gauss_LSB_Z  = 205;
      break;
  } 
}

/**************************************************************************/
/*! 
    @brief  Gets the most recent sensor event
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
  bool readingValid = false;
  
  /* Clear the event */
  memset(event, 0, sizeof(sensors_event_t));
  
  while(!readingValid)
  {
    /* Read new data */
    read();
    
    /* Make sure the sensor isn't saturating if auto-ranging is enabled */
    if (!_autoRangeEnabled)
    {
      readingValid = true;
    }
    else
    {
      serial.printf("%f \r\n",_magData.x);
      serial.printf("%f \r\n",_magData.y);
      serial.printf("%f \r\n",_magData.z);
      /* Check if the sensor is saturating or not */
      if ( (_magData.x >= 2040) | (_magData.x <= -2040) | 
           (_magData.y >= 2040) | (_magData.y <= -2040) | 
           (_magData.z >= 2040) | (_magData.z <= -2040) )
      {
        /* Saturating .... increase the range if we can */
        switch(_magGain)
        {
          case LSM303_MAGGAIN_5_6:
            setMagGain(LSM303_MAGGAIN_8_1);
            readingValid = false;
            serial.printf("Changing range to +/- 8.1 Gauss");
            break;
          case LSM303_MAGGAIN_4_7:
            setMagGain(LSM303_MAGGAIN_5_6);
            readingValid = false;
            serial.printf("Changing range to +/- 5.6 Gauss");
            break;
          case LSM303_MAGGAIN_4_0:
            setMagGain(LSM303_MAGGAIN_4_7);
            readingValid = false;
            serial.printf("Changing range to +/- 4.7 Gauss");
            break;
          case LSM303_MAGGAIN_2_5:
            setMagGain(LSM303_MAGGAIN_4_0);
            readingValid = false;
            serial.printf("Changing range to +/- 4.0 Gauss");
            break;
          case LSM303_MAGGAIN_1_9:
            setMagGain(LSM303_MAGGAIN_2_5);
            readingValid = false;
            serial.printf("Changing range to +/- 2.5 Gauss");
            break;
          case LSM303_MAGGAIN_1_3:
            setMagGain(LSM303_MAGGAIN_1_9);
            readingValid = false;
            serial.printf("Changing range to +/- 1.9");
            break;
          default:
            readingValid = true;
            break;  
        }
      }
      else
      {
        /* All values are withing range */
        readingValid = true;
      }
    }
  }
  
  event->version   = sizeof(sensors_event_t);
  event->sensor_id = _sensorID;
  event->type      = SENSOR_TYPE_MAGNETIC_FIELD;
  event->timestamp = millis();
  event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
  event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
  event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
}

/**************************************************************************/
/*! 
    @brief  Gets the sensor_t data
*/
/**************************************************************************/
void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
  /* Clear the sensor_t object */
  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */
  strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
  sensor->name[sizeof(sensor->name)- 1] = 0;
  sensor->version     = 1;
  sensor->sensor_id   = _sensorID;
  sensor->type        = SENSOR_TYPE_MAGNETIC_FIELD;
  sensor->min_delay   = 0;
  sensor->max_value   = 0.0F; // TBD
  sensor->min_value   = 0.0F; // TBD
  sensor->resolution  = 0.0F; // TBD
}