David Lakata
/
Adafruit9-DOf
Port of Adafruit Arduino code
Fork of Adafruit9-DOf by
Source/Adafruit_LSM303_U.cpp
- Committer:
- bmanga95
- Date:
- 2015-03-21
- Revision:
- 0:772bf4786416
File content as of revision 0:772bf4786416:
/*************************************************************************** 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 }