Team_temp / VT1_domc

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev

Fork of Adafruit9-DOf_AHRS_Regler_Discrete by RT-Labor IMS

Source/Adafruit_LSM303_U.cpp@1:8c4f93e10af3, 2017-10-19 (annotated)

Committer:
domanpie
Date:
Thu Oct 19 11:50:04 2017 +0000
Revision:
1:8c4f93e10af3
Parent:
0:772bf4786416 
V1.0; domc

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
domanpie 1:8c4f93e10af3 24 Serial serial( USBTX, USBRX);
domanpie 1:8c4f93e10af3 25
bmanga95 0:772bf4786416 26 /***************************************************************************
bmanga95 0:772bf4786416 27 ACCELEROMETER
bmanga95 0:772bf4786416 28 ***************************************************************************/
bmanga95 0:772bf4786416 29 /***************************************************************************
bmanga95 0:772bf4786416 30 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 31 ***************************************************************************/
bmanga95 0:772bf4786416 32
bmanga95 0:772bf4786416 33 /**************************************************************************/
bmanga95 0:772bf4786416 34 /*!
bmanga95 0:772bf4786416 35 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 36 */
bmanga95 0:772bf4786416 37 /**************************************************************************/
bmanga95 0:772bf4786416 38 void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
bmanga95 0:772bf4786416 39 {
bmanga95 0:772bf4786416 40 byte data[2] = {reg, value};
bmanga95 0:772bf4786416 41 i2c->write(address,data,2);
bmanga95 0:772bf4786416 42 }
bmanga95 0:772bf4786416 43
bmanga95 0:772bf4786416 44 /**************************************************************************/
bmanga95 0:772bf4786416 45 /*!
bmanga95 0:772bf4786416 46 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 47 */
bmanga95 0:772bf4786416 48 /**************************************************************************/
bmanga95 0:772bf4786416 49 byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
bmanga95 0:772bf4786416 50 {
bmanga95 0:772bf4786416 51 byte value;
bmanga95 0:772bf4786416 52
bmanga95 0:772bf4786416 53 i2c->writeByte(address, reg);
bmanga95 0:772bf4786416 54 i2c->read(address, &value, 1);
bmanga95 0:772bf4786416 55
bmanga95 0:772bf4786416 56 return value;
bmanga95 0:772bf4786416 57 }
bmanga95 0:772bf4786416 58
bmanga95 0:772bf4786416 59 /**************************************************************************/
bmanga95 0:772bf4786416 60 /*!
bmanga95 0:772bf4786416 61 @brief Reads the raw data from the sensor
bmanga95 0:772bf4786416 62 */
bmanga95 0:772bf4786416 63 /**************************************************************************/
bmanga95 0:772bf4786416 64 void Adafruit_LSM303_Accel_Unified::read()
bmanga95 0:772bf4786416 65 {
bmanga95 0:772bf4786416 66 // Read the accelerometer
domanpie 1:8c4f93e10af3 67 // OUT_X_L_A: X-axis acceleration data. The value is expressed in 2’s complement
bmanga95 0:772bf4786416 68 i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
bmanga95 0:772bf4786416 69
bmanga95 0:772bf4786416 70 byte data[6];
bmanga95 0:772bf4786416 71 i2c->read(LSM303_ADDRESS_ACCEL, data, 6);
bmanga95 0:772bf4786416 72
bmanga95 0:772bf4786416 73 // Shift values to create properly formed integer (low byte first)
domanpie 1:8c4f93e10af3 74 _accelData.x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
domanpie 1:8c4f93e10af3 75 raw.x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
domanpie 1:8c4f93e10af3 76
domanpie 1:8c4f93e10af3 77 _accelData.y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
domanpie 1:8c4f93e10af3 78 raw.y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
domanpie 1:8c4f93e10af3 79
domanpie 1:8c4f93e10af3 80 _accelData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
domanpie 1:8c4f93e10af3 81 raw.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
bmanga95 0:772bf4786416 82 }
bmanga95 0:772bf4786416 83
bmanga95 0:772bf4786416 84 /***************************************************************************
bmanga95 0:772bf4786416 85 CONSTRUCTOR
bmanga95 0:772bf4786416 86 ***************************************************************************/
bmanga95 0:772bf4786416 87
bmanga95 0:772bf4786416 88 /**************************************************************************/
bmanga95 0:772bf4786416 89 /*!
bmanga95 0:772bf4786416 90 @brief Instantiates a new Adafruit_LSM303 class
bmanga95 0:772bf4786416 91 */
bmanga95 0:772bf4786416 92 /**************************************************************************/
bmanga95 0:772bf4786416 93 Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
bmanga95 0:772bf4786416 94 _sensorID = sensorID;
bmanga95 0:772bf4786416 95 }
bmanga95 0:772bf4786416 96
bmanga95 0:772bf4786416 97 /***************************************************************************
bmanga95 0:772bf4786416 98 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 99 ***************************************************************************/
bmanga95 0:772bf4786416 100
bmanga95 0:772bf4786416 101 /**************************************************************************/
bmanga95 0:772bf4786416 102 /*!
bmanga95 0:772bf4786416 103 @brief Setups the HW
bmanga95 0:772bf4786416 104 */
bmanga95 0:772bf4786416 105 /**************************************************************************/
bmanga95 0:772bf4786416 106 bool Adafruit_LSM303_Accel_Unified::begin()
bmanga95 0:772bf4786416 107 {
bmanga95 0:772bf4786416 108 // Enable I2C
domanpie 1:8c4f93e10af3 109 // Enable the accelerometer (200Hz)
domanpie 1:8c4f93e10af3 110 write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x67);
bmanga95 0:772bf4786416 111
bmanga95 0:772bf4786416 112 // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
bmanga95 0:772bf4786416 113 // if we are connected or not
bmanga95 0:772bf4786416 114 uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
domanpie 1:8c4f93e10af3 115 serial.printf("LSM303_REGISTER_ACCEL_CTRL_REG1_A %d \r\n", reg1_a);
domanpie 1:8c4f93e10af3 116 if (reg1_a != 0x67)
bmanga95 0:772bf4786416 117 {
bmanga95 0:772bf4786416 118 return false;
domanpie 1:8c4f93e10af3 119 }
domanpie 1:8c4f93e10af3 120
domanpie 1:8c4f93e10af3 121 //400 Hz; normal power mode; X/Y/Z enabled
domanpie 1:8c4f93e10af3 122 write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0b01110111);
bmanga95 0:772bf4786416 123
bmanga95 0:772bf4786416 124 return true;
bmanga95 0:772bf4786416 125 }
bmanga95 0:772bf4786416 126
bmanga95 0:772bf4786416 127 /**************************************************************************/
bmanga95 0:772bf4786416 128 /*!
bmanga95 0:772bf4786416 129 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 130 */
bmanga95 0:772bf4786416 131 /**************************************************************************/
bmanga95 0:772bf4786416 132 void Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
bmanga95 0:772bf4786416 133 /* Clear the event */
bmanga95 0:772bf4786416 134 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 135
bmanga95 0:772bf4786416 136 /* Read new data */
bmanga95 0:772bf4786416 137 read();
bmanga95 0:772bf4786416 138
bmanga95 0:772bf4786416 139 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 140 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 141 event->type = SENSOR_TYPE_ACCELEROMETER;
bmanga95 0:772bf4786416 142 event->timestamp = millis();
bmanga95 0:772bf4786416 143 event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 144 event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 145 event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
bmanga95 0:772bf4786416 146 }
bmanga95 0:772bf4786416 147
bmanga95 0:772bf4786416 148 /**************************************************************************/
bmanga95 0:772bf4786416 149 /*!
bmanga95 0:772bf4786416 150 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 151 */
bmanga95 0:772bf4786416 152 /**************************************************************************/
bmanga95 0:772bf4786416 153 void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
bmanga95 0:772bf4786416 154 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 155 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 156
bmanga95 0:772bf4786416 157 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 158 strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 159 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 160 sensor->version = 1;
bmanga95 0:772bf4786416 161 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 162 sensor->type = SENSOR_TYPE_ACCELEROMETER;
bmanga95 0:772bf4786416 163 sensor->min_delay = 0;
bmanga95 0:772bf4786416 164 sensor->max_value = 0.0F; // TBD
bmanga95 0:772bf4786416 165 sensor->min_value = 0.0F; // TBD
bmanga95 0:772bf4786416 166 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 167 }
bmanga95 0:772bf4786416 168
bmanga95 0:772bf4786416 169 /***************************************************************************
bmanga95 0:772bf4786416 170 MAGNETOMETER
bmanga95 0:772bf4786416 171 ***************************************************************************/
bmanga95 0:772bf4786416 172 /***************************************************************************
bmanga95 0:772bf4786416 173 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 174 ***************************************************************************/
bmanga95 0:772bf4786416 175
bmanga95 0:772bf4786416 176 /**************************************************************************/
bmanga95 0:772bf4786416 177 /*!
bmanga95 0:772bf4786416 178 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 179 */
bmanga95 0:772bf4786416 180 /**************************************************************************/
bmanga95 0:772bf4786416 181 void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
bmanga95 0:772bf4786416 182 {
bmanga95 0:772bf4786416 183 byte data[2] = {reg, value};
bmanga95 0:772bf4786416 184 i2c->write(address,data,2);
bmanga95 0:772bf4786416 185 }
bmanga95 0:772bf4786416 186
bmanga95 0:772bf4786416 187 /**************************************************************************/
bmanga95 0:772bf4786416 188 /*!
bmanga95 0:772bf4786416 189 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 190 */
bmanga95 0:772bf4786416 191 /**************************************************************************/
bmanga95 0:772bf4786416 192 byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
bmanga95 0:772bf4786416 193 {
bmanga95 0:772bf4786416 194 byte value;
bmanga95 0:772bf4786416 195 i2c->writeByte(address, reg);
bmanga95 0:772bf4786416 196 i2c->read(address, &value, 1);
bmanga95 0:772bf4786416 197
bmanga95 0:772bf4786416 198 return value;
bmanga95 0:772bf4786416 199 }
bmanga95 0:772bf4786416 200
bmanga95 0:772bf4786416 201 /**************************************************************************/
bmanga95 0:772bf4786416 202 /*!
bmanga95 0:772bf4786416 203 @brief Reads the raw data from the sensor
bmanga95 0:772bf4786416 204 */
bmanga95 0:772bf4786416 205 /**************************************************************************/
bmanga95 0:772bf4786416 206 void Adafruit_LSM303_Mag_Unified::read()
bmanga95 0:772bf4786416 207 {
bmanga95 0:772bf4786416 208 // Read the magnetometer
bmanga95 0:772bf4786416 209 i2c->writeByte(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M);
bmanga95 0:772bf4786416 210
bmanga95 0:772bf4786416 211 byte data[6];
bmanga95 0:772bf4786416 212 i2c->read(LSM303_ADDRESS_MAG, data, 6);
domanpie 1:8c4f93e10af3 213
bmanga95 0:772bf4786416 214
bmanga95 0:772bf4786416 215 // Shift values to create properly formed integer (low byte first)
domanpie 1:8c4f93e10af3 216 // *0.5 da _magData.z in Sättigung geht
domanpie 1:8c4f93e10af3 217 _magData.x = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
domanpie 1:8c4f93e10af3 218 raw.x = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
domanpie 1:8c4f93e10af3 219
domanpie 1:8c4f93e10af3 220 _magData.z = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
domanpie 1:8c4f93e10af3 221 raw.z = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
domanpie 1:8c4f93e10af3 222
bmanga95 0:772bf4786416 223
domanpie 1:8c4f93e10af3 224 // uint32_t magZOverflow = ((uint16_t)data[4] | ((uint16_t)data[5] << 8));
domanpie 1:8c4f93e10af3 225 // if( (magZOverflow > 30000) && (magZOverflow < 32767) )
domanpie 1:8c4f93e10af3 226 // {
domanpie 1:8c4f93e10af3 227 // _magData.z = (int16_t)33023;
domanpie 1:8c4f93e10af3 228 // raw.z = (int16_t)33023;
domanpie 1:8c4f93e10af3 229 // }
domanpie 1:8c4f93e10af3 230 // else
domanpie 1:8c4f93e10af3 231 // {
domanpie 1:8c4f93e10af3 232 // _magData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
domanpie 1:8c4f93e10af3 233 // raw.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
domanpie 1:8c4f93e10af3 234 // }
domanpie 1:8c4f93e10af3 235
domanpie 1:8c4f93e10af3 236 _magData.y = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
domanpie 1:8c4f93e10af3 237 raw.y = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
domanpie 1:8c4f93e10af3 238
domanpie 1:8c4f93e10af3 239 //serial.printf("%i\r\n", magZOverflow );
domanpie 1:8c4f93e10af3 240
bmanga95 0:772bf4786416 241 // ToDo: Calculate orientation
bmanga95 0:772bf4786416 242 _magData.orientation = 0.0;
bmanga95 0:772bf4786416 243 }
bmanga95 0:772bf4786416 244
bmanga95 0:772bf4786416 245 /***************************************************************************
bmanga95 0:772bf4786416 246 CONSTRUCTOR
bmanga95 0:772bf4786416 247 ***************************************************************************/
bmanga95 0:772bf4786416 248
bmanga95 0:772bf4786416 249 /**************************************************************************/
bmanga95 0:772bf4786416 250 /*!
bmanga95 0:772bf4786416 251 @brief Instantiates a new Adafruit_LSM303 class
bmanga95 0:772bf4786416 252 */
bmanga95 0:772bf4786416 253 /**************************************************************************/
bmanga95 0:772bf4786416 254 Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
bmanga95 0:772bf4786416 255 _sensorID = sensorID;
bmanga95 0:772bf4786416 256 _autoRangeEnabled = false;
bmanga95 0:772bf4786416 257 }
bmanga95 0:772bf4786416 258
bmanga95 0:772bf4786416 259 /***************************************************************************
bmanga95 0:772bf4786416 260 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 261 ***************************************************************************/
bmanga95 0:772bf4786416 262
bmanga95 0:772bf4786416 263 /**************************************************************************/
bmanga95 0:772bf4786416 264 /*!
bmanga95 0:772bf4786416 265 @brief Setups the HW
bmanga95 0:772bf4786416 266 */
bmanga95 0:772bf4786416 267 /**************************************************************************/
bmanga95 0:772bf4786416 268 bool Adafruit_LSM303_Mag_Unified::begin()
bmanga95 0:772bf4786416 269 {
domanpie 1:8c4f93e10af3 270
bmanga95 0:772bf4786416 271 // Enable I2C
bmanga95 0:772bf4786416 272
bmanga95 0:772bf4786416 273
bmanga95 0:772bf4786416 274 // Enable the magnetometer
bmanga95 0:772bf4786416 275 write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
bmanga95 0:772bf4786416 276
bmanga95 0:772bf4786416 277 // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
domanpie 1:8c4f93e10af3 278 write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0x10);
bmanga95 0:772bf4786416 279 uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
domanpie 1:8c4f93e10af3 280 serial.printf("LSM303_REGISTER_MAG_CRA_REG_M %d \r\n", reg1_a);
bmanga95 0:772bf4786416 281 if (reg1_a != 0x10)
bmanga95 0:772bf4786416 282 {
bmanga95 0:772bf4786416 283 return false;
bmanga95 0:772bf4786416 284 }
bmanga95 0:772bf4786416 285
domanpie 1:8c4f93e10af3 286
domanpie 1:8c4f93e10af3 287
domanpie 1:8c4f93e10af3 288 //Temp measurement off; 220 Hz data outputrate
domanpie 1:8c4f93e10af3 289 this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0b00011100);
domanpie 1:8c4f93e10af3 290 // Set the gain to +/- 1.9 Gauss;
domanpie 1:8c4f93e10af3 291 this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, 0b01000000);
domanpie 1:8c4f93e10af3 292 _magGain = LSM303_MAGGAIN_1_9;
domanpie 1:8c4f93e10af3 293 //Continuous-conversion mode
domanpie 1:8c4f93e10af3 294 this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0b00000000);
domanpie 1:8c4f93e10af3 295
domanpie 1:8c4f93e10af3 296
domanpie 1:8c4f93e10af3 297
domanpie 1:8c4f93e10af3 298
domanpie 1:8c4f93e10af3 299
domanpie 1:8c4f93e10af3 300 uint8_t reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
domanpie 1:8c4f93e10af3 301 serial.printf("CRA_REG_M register %u \r\n", reg);
domanpie 1:8c4f93e10af3 302
domanpie 1:8c4f93e10af3 303 reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M);
domanpie 1:8c4f93e10af3 304 serial.printf("CRB_REG_M register %u \r\n", reg);
domanpie 1:8c4f93e10af3 305
domanpie 1:8c4f93e10af3 306 reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M);
domanpie 1:8c4f93e10af3 307 serial.printf("MR_REG_M register %u \r\n", reg);
bmanga95 0:772bf4786416 308
bmanga95 0:772bf4786416 309 return true;
bmanga95 0:772bf4786416 310 }
bmanga95 0:772bf4786416 311
bmanga95 0:772bf4786416 312 /**************************************************************************/
bmanga95 0:772bf4786416 313 /*!
bmanga95 0:772bf4786416 314 @brief Enables or disables auto-ranging
bmanga95 0:772bf4786416 315 */
bmanga95 0:772bf4786416 316 /**************************************************************************/
bmanga95 0:772bf4786416 317 void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
bmanga95 0:772bf4786416 318 {
bmanga95 0:772bf4786416 319 _autoRangeEnabled = enabled;
bmanga95 0:772bf4786416 320 }
bmanga95 0:772bf4786416 321
bmanga95 0:772bf4786416 322 /**************************************************************************/
bmanga95 0:772bf4786416 323 /*!
bmanga95 0:772bf4786416 324 @brief Sets the magnetometer's gain
bmanga95 0:772bf4786416 325 */
bmanga95 0:772bf4786416 326 /**************************************************************************/
bmanga95 0:772bf4786416 327 void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
bmanga95 0:772bf4786416 328 {
bmanga95 0:772bf4786416 329 write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
bmanga95 0:772bf4786416 330
bmanga95 0:772bf4786416 331 _magGain = gain;
bmanga95 0:772bf4786416 332
bmanga95 0:772bf4786416 333 switch(gain)
bmanga95 0:772bf4786416 334 {
bmanga95 0:772bf4786416 335 case LSM303_MAGGAIN_1_3:
bmanga95 0:772bf4786416 336 _lsm303Mag_Gauss_LSB_XY = 1100;
bmanga95 0:772bf4786416 337 _lsm303Mag_Gauss_LSB_Z = 980;
bmanga95 0:772bf4786416 338 break;
bmanga95 0:772bf4786416 339 case LSM303_MAGGAIN_1_9:
bmanga95 0:772bf4786416 340 _lsm303Mag_Gauss_LSB_XY = 855;
bmanga95 0:772bf4786416 341 _lsm303Mag_Gauss_LSB_Z = 760;
bmanga95 0:772bf4786416 342 break;
bmanga95 0:772bf4786416 343 case LSM303_MAGGAIN_2_5:
bmanga95 0:772bf4786416 344 _lsm303Mag_Gauss_LSB_XY = 670;
bmanga95 0:772bf4786416 345 _lsm303Mag_Gauss_LSB_Z = 600;
bmanga95 0:772bf4786416 346 break;
bmanga95 0:772bf4786416 347 case LSM303_MAGGAIN_4_0:
bmanga95 0:772bf4786416 348 _lsm303Mag_Gauss_LSB_XY = 450;
bmanga95 0:772bf4786416 349 _lsm303Mag_Gauss_LSB_Z = 400;
bmanga95 0:772bf4786416 350 break;
bmanga95 0:772bf4786416 351 case LSM303_MAGGAIN_4_7:
bmanga95 0:772bf4786416 352 _lsm303Mag_Gauss_LSB_XY = 400;
bmanga95 0:772bf4786416 353 _lsm303Mag_Gauss_LSB_Z = 355;
bmanga95 0:772bf4786416 354 break;
bmanga95 0:772bf4786416 355 case LSM303_MAGGAIN_5_6:
bmanga95 0:772bf4786416 356 _lsm303Mag_Gauss_LSB_XY = 330;
bmanga95 0:772bf4786416 357 _lsm303Mag_Gauss_LSB_Z = 295;
bmanga95 0:772bf4786416 358 break;
bmanga95 0:772bf4786416 359 case LSM303_MAGGAIN_8_1:
bmanga95 0:772bf4786416 360 _lsm303Mag_Gauss_LSB_XY = 230;
bmanga95 0:772bf4786416 361 _lsm303Mag_Gauss_LSB_Z = 205;
bmanga95 0:772bf4786416 362 break;
bmanga95 0:772bf4786416 363 }
bmanga95 0:772bf4786416 364 }
bmanga95 0:772bf4786416 365
bmanga95 0:772bf4786416 366 /**************************************************************************/
bmanga95 0:772bf4786416 367 /*!
bmanga95 0:772bf4786416 368 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 369 */
bmanga95 0:772bf4786416 370 /**************************************************************************/
bmanga95 0:772bf4786416 371 void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
bmanga95 0:772bf4786416 372 bool readingValid = false;
bmanga95 0:772bf4786416 373
bmanga95 0:772bf4786416 374 /* Clear the event */
bmanga95 0:772bf4786416 375 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 376
bmanga95 0:772bf4786416 377 while(!readingValid)
bmanga95 0:772bf4786416 378 {
bmanga95 0:772bf4786416 379 /* Read new data */
bmanga95 0:772bf4786416 380 read();
bmanga95 0:772bf4786416 381
bmanga95 0:772bf4786416 382 /* Make sure the sensor isn't saturating if auto-ranging is enabled */
bmanga95 0:772bf4786416 383 if (!_autoRangeEnabled)
bmanga95 0:772bf4786416 384 {
bmanga95 0:772bf4786416 385 readingValid = true;
bmanga95 0:772bf4786416 386 }
bmanga95 0:772bf4786416 387 else
bmanga95 0:772bf4786416 388 {
domanpie 1:8c4f93e10af3 389 serial.printf("%f \r\n",_magData.x);
domanpie 1:8c4f93e10af3 390 serial.printf("%f \r\n",_magData.y);
domanpie 1:8c4f93e10af3 391 serial.printf("%f \r\n",_magData.z);
bmanga95 0:772bf4786416 392 /* Check if the sensor is saturating or not */
bmanga95 0:772bf4786416 393 if ( (_magData.x >= 2040) | (_magData.x <= -2040) |
bmanga95 0:772bf4786416 394 (_magData.y >= 2040) | (_magData.y <= -2040) |
bmanga95 0:772bf4786416 395 (_magData.z >= 2040) | (_magData.z <= -2040) )
bmanga95 0:772bf4786416 396 {
bmanga95 0:772bf4786416 397 /* Saturating .... increase the range if we can */
bmanga95 0:772bf4786416 398 switch(_magGain)
bmanga95 0:772bf4786416 399 {
bmanga95 0:772bf4786416 400 case LSM303_MAGGAIN_5_6:
bmanga95 0:772bf4786416 401 setMagGain(LSM303_MAGGAIN_8_1);
bmanga95 0:772bf4786416 402 readingValid = false;
domanpie 1:8c4f93e10af3 403 serial.printf("Changing range to +/- 8.1 Gauss");
bmanga95 0:772bf4786416 404 break;
bmanga95 0:772bf4786416 405 case LSM303_MAGGAIN_4_7:
bmanga95 0:772bf4786416 406 setMagGain(LSM303_MAGGAIN_5_6);
bmanga95 0:772bf4786416 407 readingValid = false;
domanpie 1:8c4f93e10af3 408 serial.printf("Changing range to +/- 5.6 Gauss");
bmanga95 0:772bf4786416 409 break;
bmanga95 0:772bf4786416 410 case LSM303_MAGGAIN_4_0:
bmanga95 0:772bf4786416 411 setMagGain(LSM303_MAGGAIN_4_7);
bmanga95 0:772bf4786416 412 readingValid = false;
domanpie 1:8c4f93e10af3 413 serial.printf("Changing range to +/- 4.7 Gauss");
bmanga95 0:772bf4786416 414 break;
bmanga95 0:772bf4786416 415 case LSM303_MAGGAIN_2_5:
bmanga95 0:772bf4786416 416 setMagGain(LSM303_MAGGAIN_4_0);
bmanga95 0:772bf4786416 417 readingValid = false;
domanpie 1:8c4f93e10af3 418 serial.printf("Changing range to +/- 4.0 Gauss");
bmanga95 0:772bf4786416 419 break;
bmanga95 0:772bf4786416 420 case LSM303_MAGGAIN_1_9:
bmanga95 0:772bf4786416 421 setMagGain(LSM303_MAGGAIN_2_5);
bmanga95 0:772bf4786416 422 readingValid = false;
domanpie 1:8c4f93e10af3 423 serial.printf("Changing range to +/- 2.5 Gauss");
bmanga95 0:772bf4786416 424 break;
bmanga95 0:772bf4786416 425 case LSM303_MAGGAIN_1_3:
bmanga95 0:772bf4786416 426 setMagGain(LSM303_MAGGAIN_1_9);
bmanga95 0:772bf4786416 427 readingValid = false;
domanpie 1:8c4f93e10af3 428 serial.printf("Changing range to +/- 1.9");
bmanga95 0:772bf4786416 429 break;
bmanga95 0:772bf4786416 430 default:
bmanga95 0:772bf4786416 431 readingValid = true;
bmanga95 0:772bf4786416 432 break;
bmanga95 0:772bf4786416 433 }
bmanga95 0:772bf4786416 434 }
bmanga95 0:772bf4786416 435 else
bmanga95 0:772bf4786416 436 {
bmanga95 0:772bf4786416 437 /* All values are withing range */
bmanga95 0:772bf4786416 438 readingValid = true;
bmanga95 0:772bf4786416 439 }
bmanga95 0:772bf4786416 440 }
bmanga95 0:772bf4786416 441 }
bmanga95 0:772bf4786416 442
bmanga95 0:772bf4786416 443 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 444 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 445 event->type = SENSOR_TYPE_MAGNETIC_FIELD;
bmanga95 0:772bf4786416 446 event->timestamp = millis();
bmanga95 0:772bf4786416 447 event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 448 event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 449 event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
bmanga95 0:772bf4786416 450 }
bmanga95 0:772bf4786416 451
bmanga95 0:772bf4786416 452 /**************************************************************************/
bmanga95 0:772bf4786416 453 /*!
bmanga95 0:772bf4786416 454 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 455 */
bmanga95 0:772bf4786416 456 /**************************************************************************/
bmanga95 0:772bf4786416 457 void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
bmanga95 0:772bf4786416 458 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 459 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 460
bmanga95 0:772bf4786416 461 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 462 strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 463 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 464 sensor->version = 1;
bmanga95 0:772bf4786416 465 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 466 sensor->type = SENSOR_TYPE_MAGNETIC_FIELD;
bmanga95 0:772bf4786416 467 sensor->min_delay = 0;
bmanga95 0:772bf4786416 468 sensor->max_value = 0.0F; // TBD
bmanga95 0:772bf4786416 469 sensor->min_value = 0.0F; // TBD
bmanga95 0:772bf4786416 470 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 471 }