Jatin Sharma / Mbed 2 deprecated RaheeNew

RaheeNew

Dependencies:   mbed

Dependents:   RaheeNew

Fork of Adafruit9-DOf by Bruno Manganelli

Source/Adafruit_LSM303_U.cpp@0:772bf4786416, 2015-03-21 (annotated)

Committer:
bmanga95
Date:
Sat Mar 21 12:33:05 2015 +0000
Revision:
0:772bf4786416
Child:
1:c3381056a1c6
First version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmanga95 0:772bf4786416 1 /***************************************************************************
bmanga95 0:772bf4786416 2 This is a library for the LSM303 Accelerometer and magnentometer/compass
bmanga95 0:772bf4786416 3
bmanga95 0:772bf4786416 4 Designed specifically to work with the Adafruit LSM303DLHC Breakout
bmanga95 0:772bf4786416 5
bmanga95 0:772bf4786416 6 These displays use I2C to communicate, 2 pins are required to interface.
bmanga95 0:772bf4786416 7
bmanga95 0:772bf4786416 8 Adafruit invests time and resources providing this open source code,
bmanga95 0:772bf4786416 9 please support Adafruit andopen-source hardware by purchasing products
bmanga95 0:772bf4786416 10 from Adafruit!
bmanga95 0:772bf4786416 11
bmanga95 0:772bf4786416 12 Written by Kevin Townsend for Adafruit Industries.
bmanga95 0:772bf4786416 13 BSD license, all text above must be included in any redistribution
bmanga95 0:772bf4786416 14 ***************************************************************************/
bmanga95 0:772bf4786416 15
bmanga95 0:772bf4786416 16
bmanga95 0:772bf4786416 17 #include "Adafruit_LSM303_U.h"
bmanga95 0:772bf4786416 18 #include "Serial_base.h"
bmanga95 0:772bf4786416 19
bmanga95 0:772bf4786416 20 static float _lsm303Accel_MG_LSB = 0.001F; // 1, 2, 4 or 12 mg per lsb
bmanga95 0:772bf4786416 21 static float _lsm303Mag_Gauss_LSB_XY = 1100.0F; // Varies with gain
bmanga95 0:772bf4786416 22 static float _lsm303Mag_Gauss_LSB_Z = 980.0F; // Varies with gain
bmanga95 0:772bf4786416 23
bmanga95 0:772bf4786416 24 /***************************************************************************
bmanga95 0:772bf4786416 25 ACCELEROMETER
bmanga95 0:772bf4786416 26 ***************************************************************************/
bmanga95 0:772bf4786416 27 /***************************************************************************
bmanga95 0:772bf4786416 28 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 29 ***************************************************************************/
bmanga95 0:772bf4786416 30
bmanga95 0:772bf4786416 31 /**************************************************************************/
bmanga95 0:772bf4786416 32 /*!
bmanga95 0:772bf4786416 33 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 34 */
bmanga95 0:772bf4786416 35 /**************************************************************************/
bmanga95 0:772bf4786416 36 void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
bmanga95 0:772bf4786416 37 {
bmanga95 0:772bf4786416 38 byte data[2] = {reg, value};
bmanga95 0:772bf4786416 39 i2c->write(address,data,2);
bmanga95 0:772bf4786416 40 }
bmanga95 0:772bf4786416 41
bmanga95 0:772bf4786416 42 /**************************************************************************/
bmanga95 0:772bf4786416 43 /*!
bmanga95 0:772bf4786416 44 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 45 */
bmanga95 0:772bf4786416 46 /**************************************************************************/
bmanga95 0:772bf4786416 47 byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
bmanga95 0:772bf4786416 48 {
bmanga95 0:772bf4786416 49 byte value;
bmanga95 0:772bf4786416 50
bmanga95 0:772bf4786416 51 i2c->writeByte(address, reg);
bmanga95 0:772bf4786416 52 i2c->read(address, &value, 1);
bmanga95 0:772bf4786416 53
bmanga95 0:772bf4786416 54 return value;
bmanga95 0:772bf4786416 55 }
bmanga95 0:772bf4786416 56
bmanga95 0:772bf4786416 57 /**************************************************************************/
bmanga95 0:772bf4786416 58 /*!
bmanga95 0:772bf4786416 59 @brief Reads the raw data from the sensor
bmanga95 0:772bf4786416 60 */
bmanga95 0:772bf4786416 61 /**************************************************************************/
bmanga95 0:772bf4786416 62 void Adafruit_LSM303_Accel_Unified::read()
bmanga95 0:772bf4786416 63 {
bmanga95 0:772bf4786416 64 // Read the accelerometer
bmanga95 0:772bf4786416 65 i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
bmanga95 0:772bf4786416 66
bmanga95 0:772bf4786416 67 byte data[6];
bmanga95 0:772bf4786416 68 i2c->read(LSM303_ADDRESS_ACCEL, data, 6);
bmanga95 0:772bf4786416 69
bmanga95 0:772bf4786416 70 // Shift values to create properly formed integer (low byte first)
bmanga95 0:772bf4786416 71 _accelData.x = (int16_t)(data[0] | (data[1] << 8)) >> 4;
bmanga95 0:772bf4786416 72 _accelData.y = (int16_t)(data[2] | (data[3] << 8)) >> 4;
bmanga95 0:772bf4786416 73 _accelData.z = (int16_t)(data[4] | (data[5] << 8)) >> 4;
bmanga95 0:772bf4786416 74 }
bmanga95 0:772bf4786416 75
bmanga95 0:772bf4786416 76 /***************************************************************************
bmanga95 0:772bf4786416 77 CONSTRUCTOR
bmanga95 0:772bf4786416 78 ***************************************************************************/
bmanga95 0:772bf4786416 79
bmanga95 0:772bf4786416 80 /**************************************************************************/
bmanga95 0:772bf4786416 81 /*!
bmanga95 0:772bf4786416 82 @brief Instantiates a new Adafruit_LSM303 class
bmanga95 0:772bf4786416 83 */
bmanga95 0:772bf4786416 84 /**************************************************************************/
bmanga95 0:772bf4786416 85 Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
bmanga95 0:772bf4786416 86 _sensorID = sensorID;
bmanga95 0:772bf4786416 87 }
bmanga95 0:772bf4786416 88
bmanga95 0:772bf4786416 89 /***************************************************************************
bmanga95 0:772bf4786416 90 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 91 ***************************************************************************/
bmanga95 0:772bf4786416 92
bmanga95 0:772bf4786416 93 /**************************************************************************/
bmanga95 0:772bf4786416 94 /*!
bmanga95 0:772bf4786416 95 @brief Setups the HW
bmanga95 0:772bf4786416 96 */
bmanga95 0:772bf4786416 97 /**************************************************************************/
bmanga95 0:772bf4786416 98 bool Adafruit_LSM303_Accel_Unified::begin()
bmanga95 0:772bf4786416 99 {
bmanga95 0:772bf4786416 100 // Enable I2C
bmanga95 0:772bf4786416 101 // Enable the accelerometer (100Hz)
bmanga95 0:772bf4786416 102 write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
bmanga95 0:772bf4786416 103
bmanga95 0:772bf4786416 104 // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
bmanga95 0:772bf4786416 105 // if we are connected or not
bmanga95 0:772bf4786416 106 uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
bmanga95 0:772bf4786416 107 if (reg1_a != 0x57)
bmanga95 0:772bf4786416 108 {
bmanga95 0:772bf4786416 109 return false;
bmanga95 0:772bf4786416 110 }
bmanga95 0:772bf4786416 111
bmanga95 0:772bf4786416 112 return true;
bmanga95 0:772bf4786416 113 }
bmanga95 0:772bf4786416 114
bmanga95 0:772bf4786416 115 /**************************************************************************/
bmanga95 0:772bf4786416 116 /*!
bmanga95 0:772bf4786416 117 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 118 */
bmanga95 0:772bf4786416 119 /**************************************************************************/
bmanga95 0:772bf4786416 120 void Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
bmanga95 0:772bf4786416 121 /* Clear the event */
bmanga95 0:772bf4786416 122 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 123
bmanga95 0:772bf4786416 124 /* Read new data */
bmanga95 0:772bf4786416 125 read();
bmanga95 0:772bf4786416 126
bmanga95 0:772bf4786416 127 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 128 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 129 event->type = SENSOR_TYPE_ACCELEROMETER;
bmanga95 0:772bf4786416 130 event->timestamp = millis();
bmanga95 0:772bf4786416 131 event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 132 event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 133 event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 134 }
bmanga95 0:772bf4786416 135
bmanga95 0:772bf4786416 136 /**************************************************************************/
bmanga95 0:772bf4786416 137 /*!
bmanga95 0:772bf4786416 138 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 139 */
bmanga95 0:772bf4786416 140 /**************************************************************************/
bmanga95 0:772bf4786416 141 void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
bmanga95 0:772bf4786416 142 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 143 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 144
bmanga95 0:772bf4786416 145 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 146 strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 147 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 148 sensor->version = 1;
bmanga95 0:772bf4786416 149 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 150 sensor->type = SENSOR_TYPE_ACCELEROMETER;
bmanga95 0:772bf4786416 151 sensor->min_delay = 0;
bmanga95 0:772bf4786416 152 sensor->max_value = 0.0F; // TBD
bmanga95 0:772bf4786416 153 sensor->min_value = 0.0F; // TBD
bmanga95 0:772bf4786416 154 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 155 }
bmanga95 0:772bf4786416 156
bmanga95 0:772bf4786416 157 /***************************************************************************
bmanga95 0:772bf4786416 158 MAGNETOMETER
bmanga95 0:772bf4786416 159 ***************************************************************************/
bmanga95 0:772bf4786416 160 /***************************************************************************
bmanga95 0:772bf4786416 161 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 162 ***************************************************************************/
bmanga95 0:772bf4786416 163
bmanga95 0:772bf4786416 164 /**************************************************************************/
bmanga95 0:772bf4786416 165 /*!
bmanga95 0:772bf4786416 166 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 167 */
bmanga95 0:772bf4786416 168 /**************************************************************************/
bmanga95 0:772bf4786416 169 void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
bmanga95 0:772bf4786416 170 {
bmanga95 0:772bf4786416 171 byte data[2] = {reg, value};
bmanga95 0:772bf4786416 172 i2c->write(address,data,2);
bmanga95 0:772bf4786416 173 }
bmanga95 0:772bf4786416 174
bmanga95 0:772bf4786416 175 /**************************************************************************/
bmanga95 0:772bf4786416 176 /*!
bmanga95 0:772bf4786416 177 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 178 */
bmanga95 0:772bf4786416 179 /**************************************************************************/
bmanga95 0:772bf4786416 180 byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
bmanga95 0:772bf4786416 181 {
bmanga95 0:772bf4786416 182 byte value;
bmanga95 0:772bf4786416 183 i2c->writeByte(address, reg);
bmanga95 0:772bf4786416 184 i2c->read(address, &value, 1);
bmanga95 0:772bf4786416 185
bmanga95 0:772bf4786416 186 return value;
bmanga95 0:772bf4786416 187 }
bmanga95 0:772bf4786416 188
bmanga95 0:772bf4786416 189 /**************************************************************************/
bmanga95 0:772bf4786416 190 /*!
bmanga95 0:772bf4786416 191 @brief Reads the raw data from the sensor
bmanga95 0:772bf4786416 192 */
bmanga95 0:772bf4786416 193 /**************************************************************************/
bmanga95 0:772bf4786416 194 void Adafruit_LSM303_Mag_Unified::read()
bmanga95 0:772bf4786416 195 {
bmanga95 0:772bf4786416 196 // Read the magnetometer
bmanga95 0:772bf4786416 197 i2c->writeByte(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M);
bmanga95 0:772bf4786416 198
bmanga95 0:772bf4786416 199 byte data[6];
bmanga95 0:772bf4786416 200 i2c->read(LSM303_ADDRESS_MAG, data, 6);
bmanga95 0:772bf4786416 201
bmanga95 0:772bf4786416 202
bmanga95 0:772bf4786416 203 // Shift values to create properly formed integer (low byte first)
bmanga95 0:772bf4786416 204 _magData.x = (int16_t)(data[0] | ((int16_t)data[1] << 8));
bmanga95 0:772bf4786416 205 _magData.y = (int16_t)(data[2] | ((int16_t)data[3] << 8));
bmanga95 0:772bf4786416 206 _magData.z = (int16_t)(data[4] | ((int16_t)data[5] << 8));
bmanga95 0:772bf4786416 207
bmanga95 0:772bf4786416 208 // ToDo: Calculate orientation
bmanga95 0:772bf4786416 209 _magData.orientation = 0.0;
bmanga95 0:772bf4786416 210 }
bmanga95 0:772bf4786416 211
bmanga95 0:772bf4786416 212 /***************************************************************************
bmanga95 0:772bf4786416 213 CONSTRUCTOR
bmanga95 0:772bf4786416 214 ***************************************************************************/
bmanga95 0:772bf4786416 215
bmanga95 0:772bf4786416 216 /**************************************************************************/
bmanga95 0:772bf4786416 217 /*!
bmanga95 0:772bf4786416 218 @brief Instantiates a new Adafruit_LSM303 class
bmanga95 0:772bf4786416 219 */
bmanga95 0:772bf4786416 220 /**************************************************************************/
bmanga95 0:772bf4786416 221 Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
bmanga95 0:772bf4786416 222 _sensorID = sensorID;
bmanga95 0:772bf4786416 223 _autoRangeEnabled = false;
bmanga95 0:772bf4786416 224 }
bmanga95 0:772bf4786416 225
bmanga95 0:772bf4786416 226 /***************************************************************************
bmanga95 0:772bf4786416 227 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 228 ***************************************************************************/
bmanga95 0:772bf4786416 229
bmanga95 0:772bf4786416 230 /**************************************************************************/
bmanga95 0:772bf4786416 231 /*!
bmanga95 0:772bf4786416 232 @brief Setups the HW
bmanga95 0:772bf4786416 233 */
bmanga95 0:772bf4786416 234 /**************************************************************************/
bmanga95 0:772bf4786416 235 bool Adafruit_LSM303_Mag_Unified::begin()
bmanga95 0:772bf4786416 236 {
bmanga95 0:772bf4786416 237 // Enable I2C
bmanga95 0:772bf4786416 238
bmanga95 0:772bf4786416 239
bmanga95 0:772bf4786416 240 // Enable the magnetometer
bmanga95 0:772bf4786416 241 write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
bmanga95 0:772bf4786416 242
bmanga95 0:772bf4786416 243 // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
bmanga95 0:772bf4786416 244 // the default value (0b00010000/0x10)
bmanga95 0:772bf4786416 245 uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
bmanga95 0:772bf4786416 246 if (reg1_a != 0x10)
bmanga95 0:772bf4786416 247 {
bmanga95 0:772bf4786416 248 return false;
bmanga95 0:772bf4786416 249 }
bmanga95 0:772bf4786416 250
bmanga95 0:772bf4786416 251 // Set the gain to a known level
bmanga95 0:772bf4786416 252 setMagGain(LSM303_MAGGAIN_1_3);
bmanga95 0:772bf4786416 253
bmanga95 0:772bf4786416 254 return true;
bmanga95 0:772bf4786416 255 }
bmanga95 0:772bf4786416 256
bmanga95 0:772bf4786416 257 /**************************************************************************/
bmanga95 0:772bf4786416 258 /*!
bmanga95 0:772bf4786416 259 @brief Enables or disables auto-ranging
bmanga95 0:772bf4786416 260 */
bmanga95 0:772bf4786416 261 /**************************************************************************/
bmanga95 0:772bf4786416 262 void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
bmanga95 0:772bf4786416 263 {
bmanga95 0:772bf4786416 264 _autoRangeEnabled = enabled;
bmanga95 0:772bf4786416 265 }
bmanga95 0:772bf4786416 266
bmanga95 0:772bf4786416 267 /**************************************************************************/
bmanga95 0:772bf4786416 268 /*!
bmanga95 0:772bf4786416 269 @brief Sets the magnetometer's gain
bmanga95 0:772bf4786416 270 */
bmanga95 0:772bf4786416 271 /**************************************************************************/
bmanga95 0:772bf4786416 272 void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
bmanga95 0:772bf4786416 273 {
bmanga95 0:772bf4786416 274 write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
bmanga95 0:772bf4786416 275
bmanga95 0:772bf4786416 276 _magGain = gain;
bmanga95 0:772bf4786416 277
bmanga95 0:772bf4786416 278 switch(gain)
bmanga95 0:772bf4786416 279 {
bmanga95 0:772bf4786416 280 case LSM303_MAGGAIN_1_3:
bmanga95 0:772bf4786416 281 _lsm303Mag_Gauss_LSB_XY = 1100;
bmanga95 0:772bf4786416 282 _lsm303Mag_Gauss_LSB_Z = 980;
bmanga95 0:772bf4786416 283 break;
bmanga95 0:772bf4786416 284 case LSM303_MAGGAIN_1_9:
bmanga95 0:772bf4786416 285 _lsm303Mag_Gauss_LSB_XY = 855;
bmanga95 0:772bf4786416 286 _lsm303Mag_Gauss_LSB_Z = 760;
bmanga95 0:772bf4786416 287 break;
bmanga95 0:772bf4786416 288 case LSM303_MAGGAIN_2_5:
bmanga95 0:772bf4786416 289 _lsm303Mag_Gauss_LSB_XY = 670;
bmanga95 0:772bf4786416 290 _lsm303Mag_Gauss_LSB_Z = 600;
bmanga95 0:772bf4786416 291 break;
bmanga95 0:772bf4786416 292 case LSM303_MAGGAIN_4_0:
bmanga95 0:772bf4786416 293 _lsm303Mag_Gauss_LSB_XY = 450;
bmanga95 0:772bf4786416 294 _lsm303Mag_Gauss_LSB_Z = 400;
bmanga95 0:772bf4786416 295 break;
bmanga95 0:772bf4786416 296 case LSM303_MAGGAIN_4_7:
bmanga95 0:772bf4786416 297 _lsm303Mag_Gauss_LSB_XY = 400;
bmanga95 0:772bf4786416 298 _lsm303Mag_Gauss_LSB_Z = 355;
bmanga95 0:772bf4786416 299 break;
bmanga95 0:772bf4786416 300 case LSM303_MAGGAIN_5_6:
bmanga95 0:772bf4786416 301 _lsm303Mag_Gauss_LSB_XY = 330;
bmanga95 0:772bf4786416 302 _lsm303Mag_Gauss_LSB_Z = 295;
bmanga95 0:772bf4786416 303 break;
bmanga95 0:772bf4786416 304 case LSM303_MAGGAIN_8_1:
bmanga95 0:772bf4786416 305 _lsm303Mag_Gauss_LSB_XY = 230;
bmanga95 0:772bf4786416 306 _lsm303Mag_Gauss_LSB_Z = 205;
bmanga95 0:772bf4786416 307 break;
bmanga95 0:772bf4786416 308 }
bmanga95 0:772bf4786416 309 }
bmanga95 0:772bf4786416 310
bmanga95 0:772bf4786416 311 /**************************************************************************/
bmanga95 0:772bf4786416 312 /*!
bmanga95 0:772bf4786416 313 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 314 */
bmanga95 0:772bf4786416 315 /**************************************************************************/
bmanga95 0:772bf4786416 316 void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
bmanga95 0:772bf4786416 317 bool readingValid = false;
bmanga95 0:772bf4786416 318
bmanga95 0:772bf4786416 319 /* Clear the event */
bmanga95 0:772bf4786416 320 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 321
bmanga95 0:772bf4786416 322 while(!readingValid)
bmanga95 0:772bf4786416 323 {
bmanga95 0:772bf4786416 324 /* Read new data */
bmanga95 0:772bf4786416 325 read();
bmanga95 0:772bf4786416 326
bmanga95 0:772bf4786416 327 /* Make sure the sensor isn't saturating if auto-ranging is enabled */
bmanga95 0:772bf4786416 328 if (!_autoRangeEnabled)
bmanga95 0:772bf4786416 329 {
bmanga95 0:772bf4786416 330 readingValid = true;
bmanga95 0:772bf4786416 331 }
bmanga95 0:772bf4786416 332 else
bmanga95 0:772bf4786416 333 {
bmanga95 0:772bf4786416 334 s_com->print(_magData.x); s_com->print(" ");
bmanga95 0:772bf4786416 335 s_com->print(_magData.y); s_com->print(" ");
bmanga95 0:772bf4786416 336 s_com->print(_magData.z); s_com->println(" ");
bmanga95 0:772bf4786416 337 /* Check if the sensor is saturating or not */
bmanga95 0:772bf4786416 338 if ( (_magData.x >= 2040) | (_magData.x <= -2040) |
bmanga95 0:772bf4786416 339 (_magData.y >= 2040) | (_magData.y <= -2040) |
bmanga95 0:772bf4786416 340 (_magData.z >= 2040) | (_magData.z <= -2040) )
bmanga95 0:772bf4786416 341 {
bmanga95 0:772bf4786416 342 /* Saturating .... increase the range if we can */
bmanga95 0:772bf4786416 343 switch(_magGain)
bmanga95 0:772bf4786416 344 {
bmanga95 0:772bf4786416 345 case LSM303_MAGGAIN_5_6:
bmanga95 0:772bf4786416 346 setMagGain(LSM303_MAGGAIN_8_1);
bmanga95 0:772bf4786416 347 readingValid = false;
bmanga95 0:772bf4786416 348 s_com->println("Changing range to +/- 8.1");
bmanga95 0:772bf4786416 349 break;
bmanga95 0:772bf4786416 350 case LSM303_MAGGAIN_4_7:
bmanga95 0:772bf4786416 351 setMagGain(LSM303_MAGGAIN_5_6);
bmanga95 0:772bf4786416 352 readingValid = false;
bmanga95 0:772bf4786416 353 s_com->println("Changing range to +/- 5.6");
bmanga95 0:772bf4786416 354 break;
bmanga95 0:772bf4786416 355 case LSM303_MAGGAIN_4_0:
bmanga95 0:772bf4786416 356 setMagGain(LSM303_MAGGAIN_4_7);
bmanga95 0:772bf4786416 357 readingValid = false;
bmanga95 0:772bf4786416 358 s_com->println("Changing range to +/- 4.7");
bmanga95 0:772bf4786416 359 break;
bmanga95 0:772bf4786416 360 case LSM303_MAGGAIN_2_5:
bmanga95 0:772bf4786416 361 setMagGain(LSM303_MAGGAIN_4_0);
bmanga95 0:772bf4786416 362 readingValid = false;
bmanga95 0:772bf4786416 363 s_com->println("Changing range to +/- 4.0");
bmanga95 0:772bf4786416 364 break;
bmanga95 0:772bf4786416 365 case LSM303_MAGGAIN_1_9:
bmanga95 0:772bf4786416 366 setMagGain(LSM303_MAGGAIN_2_5);
bmanga95 0:772bf4786416 367 readingValid = false;
bmanga95 0:772bf4786416 368 s_com->println("Changing range to +/- 2.5");
bmanga95 0:772bf4786416 369 break;
bmanga95 0:772bf4786416 370 case LSM303_MAGGAIN_1_3:
bmanga95 0:772bf4786416 371 setMagGain(LSM303_MAGGAIN_1_9);
bmanga95 0:772bf4786416 372 readingValid = false;
bmanga95 0:772bf4786416 373 s_com->println("Changing range to +/- 1.9");
bmanga95 0:772bf4786416 374 break;
bmanga95 0:772bf4786416 375 default:
bmanga95 0:772bf4786416 376 readingValid = true;
bmanga95 0:772bf4786416 377 break;
bmanga95 0:772bf4786416 378 }
bmanga95 0:772bf4786416 379 }
bmanga95 0:772bf4786416 380 else
bmanga95 0:772bf4786416 381 {
bmanga95 0:772bf4786416 382 /* All values are withing range */
bmanga95 0:772bf4786416 383 readingValid = true;
bmanga95 0:772bf4786416 384 }
bmanga95 0:772bf4786416 385 }
bmanga95 0:772bf4786416 386 }
bmanga95 0:772bf4786416 387
bmanga95 0:772bf4786416 388 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 389 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 390 event->type = SENSOR_TYPE_MAGNETIC_FIELD;
bmanga95 0:772bf4786416 391 event->timestamp = millis();
bmanga95 0:772bf4786416 392 event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 393 event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 394 event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 395 }
bmanga95 0:772bf4786416 396
bmanga95 0:772bf4786416 397 /**************************************************************************/
bmanga95 0:772bf4786416 398 /*!
bmanga95 0:772bf4786416 399 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 400 */
bmanga95 0:772bf4786416 401 /**************************************************************************/
bmanga95 0:772bf4786416 402 void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
bmanga95 0:772bf4786416 403 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 404 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 405
bmanga95 0:772bf4786416 406 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 407 strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 408 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 409 sensor->version = 1;
bmanga95 0:772bf4786416 410 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 411 sensor->type = SENSOR_TYPE_MAGNETIC_FIELD;
bmanga95 0:772bf4786416 412 sensor->min_delay = 0;
bmanga95 0:772bf4786416 413 sensor->max_value = 0.0F; // TBD
bmanga95 0:772bf4786416 414 sensor->min_value = 0.0F; // TBD
bmanga95 0:772bf4786416 415 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 416 }