Bruno Manganelli
Port of Adafruit Arduino code
Diff: Source/Adafruit_LSM303_U.cpp
- Revision:
- 0:772bf4786416
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_LSM303_U.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,416 @@
+/***************************************************************************
+ 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
+
+/***************************************************************************
+ 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
+ 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)(data[0] | (data[1] << 8)) >> 4;
+ _accelData.y = (int16_t)(data[2] | (data[3] << 8)) >> 4;
+ _accelData.z = (int16_t)(data[4] | (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 (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
+*/
+/**************************************************************************/
+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)
+ _magData.x = (int16_t)(data[0] | ((int16_t)data[1] << 8));
+ _magData.y = (int16_t)(data[2] | ((int16_t)data[3] << 8));
+ _magData.z = (int16_t)(data[4] | ((int16_t)data[5] << 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 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
+ // 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 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
+ {
+ s_com->print(_magData.x); s_com->print(" ");
+ s_com->print(_magData.y); s_com->print(" ");
+ s_com->print(_magData.z); s_com->println(" ");
+ /* 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;
+ s_com->println("Changing range to +/- 8.1");
+ break;
+ case LSM303_MAGGAIN_4_7:
+ setMagGain(LSM303_MAGGAIN_5_6);
+ readingValid = false;
+ s_com->println("Changing range to +/- 5.6");
+ break;
+ case LSM303_MAGGAIN_4_0:
+ setMagGain(LSM303_MAGGAIN_4_7);
+ readingValid = false;
+ s_com->println("Changing range to +/- 4.7");
+ break;
+ case LSM303_MAGGAIN_2_5:
+ setMagGain(LSM303_MAGGAIN_4_0);
+ readingValid = false;
+ s_com->println("Changing range to +/- 4.0");
+ break;
+ case LSM303_MAGGAIN_1_9:
+ setMagGain(LSM303_MAGGAIN_2_5);
+ readingValid = false;
+ s_com->println("Changing range to +/- 2.5");
+ break;
+ case LSM303_MAGGAIN_1_3:
+ setMagGain(LSM303_MAGGAIN_1_9);
+ readingValid = false;
+ s_com->println("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
+}