JB Dean / GR-PeachAHRSWeb

Modified to run on Renesas GR Peach board

Dependencies:   EthernetInterface HTTP-Server Webpage mbed-rpc mbed-src

Fork of HTTPServer_echoback by Takuya Urakawa

Adafruit_10DOF/Adafruit_LSM303_U.cpp@16:5d102be2566c, 2015-10-07 (annotated)

Committer:
webOnBoard
Date:
Wed Oct 07 20:42:51 2015 +0000
Revision:
16:5d102be2566c
web based AHRS demo Renesas GR Peach board; Using HTTP Server/RPC Adafruit 10DOF

Who changed what in which revision?

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