Diff: Adafruit_10DOF/Adafruit_LSM303_U.cpp

Revision:

16:5d102be2566c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_10DOF/Adafruit_LSM303_U.cpp	Wed Oct 07 20:42:51 2015 +0000
@@ -0,0 +1,530 @@
+/***************************************************************************
+  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"
+
+//extern I2C i2c(I2C_SDA, I2C_SCL);        // sda, scl
+
+/* enabling this #define will enable the debug print blocks
+#define LSM303_DEBUG
+*/
+typedef uint8_t byte;
+
+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
+
+/***************************************************************************
+ ACCELEROMETER
+ ***************************************************************************/
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+    @brief  Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
+{
+	char data_write[2];
+
+    data_write[0] = reg;
+    data_write[1] = value;
+    int status = i2c->write(address, data_write, 2, 0);
+
+}
+
+/**************************************************************************/
+/*!
+    @brief  Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
+{
+ 	byte value;
+
+  
+    char data_write[2];
+    char data_read[2];
+
+  
+    // Read register
+    data_write[0] = reg;
+    i2c->write(address, data_write, 1, 1); // no stop
+    i2c->read(address, data_read, 2, 0);
+    value = data_read[0];
+    
+  return value;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Reads the raw data from the sensor
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Accel_Unified::read()
+{
+	char data_write[2];
+	char data_read[6];
+	uint32_t status;
+	uint8_t xlo;
+	uint8_t xhi;
+	uint8_t ylo;
+	uint8_t yhi;
+	uint8_t zlo;
+	uint8_t zhi;
+	
+  data_write[0] = LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80 ;
+
+  i2c->write(LSM303_ADDRESS_ACCEL, data_write, 1, 1);// non stop i2c
+  
+  // Read the accelerometer
+  // Read 6 byte data
+  status = i2c->read(LSM303_ADDRESS_ACCEL, data_read, 6, 0);
+  // Read the accelerometer
+
+  // Wait around until enough data is available
+
+	if ( status == 0 )
+  	{
+    	xlo = data_read[0];
+    	xhi = data_read[1];
+    	ylo = data_read[2];
+    	yhi = data_read[3];
+    	zlo = data_read[4];
+    	zhi = data_read[5];
+
+
+  // Shift values to create properly formed integer (low byte first)
+    _accelData.x = (int16_t)(xlo | (xhi << 8)) >> 4;
+    _accelData.y = (int16_t)(ylo | (yhi << 8)) >> 4;
+    _accelData.z = (int16_t)(zlo | (zhi << 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 the accelerometer (100Hz)
+  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
+  
+  // 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);
+  if (reg1_a != 0x57)
+  {
+    return false;
+  }  
+  
+  return true;
+}
+
+/**************************************************************************/
+/*! 
+    @brief  Gets the most recent sensor event
+*/
+/**************************************************************************/
+bool 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 = timer.read_ms();//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;
+
+  return true;
+}
+
+/**************************************************************************/
+/*! 
+    @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)
+{
+    char data_write[2];
+
+    data_write[0] = reg;
+    data_write[1] = value;
+    int status = i2c->write(address, data_write, 2, 0);
+
+}
+
+/**************************************************************************/
+/*!
+    @brief  Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
+{
+  byte value;
+
+    char data_write[2];
+    char data_read[2];
+
+    // Read register
+    data_write[0] = reg;
+    i2c->write(address, data_write, 1, 1); // no stop
+    i2c->read(address, data_read, 2, 0);
+    value = data_read[0];
+    
+  return value;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Reads the raw data from the sensor
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::read()
+{
+
+  char data_write[2];
+  char data_read[6];
+  uint32_t status;      
+  uint8_t xhi;
+  uint8_t xlo;
+  uint8_t zhi;
+  uint8_t zlo;
+  uint8_t yhi;
+  uint8_t ylo;
+  // Read the magnetometer
+  data_write[0] = LSM303_REGISTER_MAG_OUT_X_H_M ;
+
+  i2c->write(LSM303_ADDRESS_MAG, data_write, 1, 1);// non stop i2c
+  
+  // Read the accelerometer
+  // Read 6 byte data
+  status = i2c->read(LSM303_ADDRESS_MAG, data_read, 6, 0);
+  // Read the accelerometer
+
+  // Wait around until enough data is available
+
+	if ( status == 0 )
+  	{
+    	xlo = data_read[0];
+    	xhi = data_read[1];
+    	ylo = data_read[2];
+    	yhi = data_read[3];
+    	zlo = data_read[4];
+    	zhi = data_read[5];
+
+
+  // Shift values to create properly formed integer (low byte first)
+  _magData.x = (int16_t)(xlo | ((int16_t)xhi << 8));
+  _magData.y = (int16_t)(ylo | ((int16_t)yhi << 8));
+  _magData.z = (int16_t)(zlo | ((int16_t)zhi << 8));
+  }  
+  // 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 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
+  // the default value (0b00010000/0x10)
+  uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
+  if (reg1_a != 0x10)
+  {
+    return false;
+  }
+
+  // Set the gain to a known level
+  setMagGain(LSM303_MAGGAIN_1_3);
+
+  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  Sets the magnetometer's update rate
+*/
+/**************************************************************************/
+void Adafruit_LSM303_Mag_Unified::setMagRate(lsm303MagRate rate)
+{
+	byte reg_m = ((byte)rate & 0x07) << 2;
+  write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, reg_m);
+}
+
+
+/**************************************************************************/
+/*! 
+    @brief  Gets the most recent sensor event
+*/
+/**************************************************************************/
+bool Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
+  bool readingValid = false;
+  
+  /* Clear the event */
+  memset(event, 0, sizeof(sensors_event_t));
+  
+  while(!readingValid)
+  {
+
+    uint8_t reg_mg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_SR_REG_Mg);
+    if (!(reg_mg & 0x1)) {
+			return false;
+    }
+  
+    /* Read new data */
+    read();
+    
+    /* Make sure the sensor isn't saturating if auto-ranging is enabled */
+    if (!_autoRangeEnabled)
+    {
+      readingValid = true;
+    }
+    else
+    {
+#ifdef LSM303_DEBUG
+      Serial.print(_magData.x); Serial.print(" ");
+      Serial.print(_magData.y); Serial.print(" ");
+      Serial.print(_magData.z); Serial.println(" ");
+#endif	  
+      /* 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;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 8.1");
+#endif
+            break;
+          case LSM303_MAGGAIN_4_7:
+            setMagGain(LSM303_MAGGAIN_5_6);
+            readingValid = false;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 5.6");
+#endif
+            break;
+          case LSM303_MAGGAIN_4_0:
+            setMagGain(LSM303_MAGGAIN_4_7);
+            readingValid = false;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 4.7");
+#endif			
+            break;
+          case LSM303_MAGGAIN_2_5:
+            setMagGain(LSM303_MAGGAIN_4_0);
+            readingValid = false;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 4.0");
+#endif			
+            break;
+          case LSM303_MAGGAIN_1_9:
+            setMagGain(LSM303_MAGGAIN_2_5);
+            readingValid = false;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 2.5");
+#endif			
+            break;
+          case LSM303_MAGGAIN_1_3:
+            setMagGain(LSM303_MAGGAIN_1_9);
+            readingValid = false;
+#ifdef LSM303_DEBUG
+            Serial.println("Changing range to +/- 1.9");
+#endif			
+            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 = timer.read_ms();//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;
+		
+	return true;
+}
+
+/**************************************************************************/
+/*! 
+    @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
+}