Hassan Elahi / LSM303D

reading from 6 sensors and packaging the data

Dependents:   LSM303D_SPI

LSM303D.cpp@2:171076b97de0, 2015-03-23 (annotated)

Committer:
hassan_elahi
Date:
Mon Mar 23 19:18:57 2015 +0000
Revision:
2:171076b97de0
Parent:
1:3948b63ff4bd 
asfdsd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DrCoyle 0:f186dd92c836 1 /** Tilt-compensated compass interface Library for the STMicro LSM303DLH 3-axis magnetometer, 3-axis acceleromter
DrCoyle 0:f186dd92c836 2 *
DrCoyle 0:f186dd92c836 3 * Written by Eric Coyle
DrCoyle 0:f186dd92c836 4 *
DrCoyle 0:f186dd92c836 5 * Based on Michael Shimniok's LSM303DLH library which is based on
DrCoyle 0:f186dd92c836 6 * test program by @tosihisa and
DrCoyle 0:f186dd92c836 7 * Pololu sample library for LSM303DLH breakout by ryantm:
DrCoyle 0:f186dd92c836 8 *
DrCoyle 0:f186dd92c836 9 * Copyright (c) 2011 Pololu Corporation. For more information, see
DrCoyle 0:f186dd92c836 10 *
DrCoyle 0:f186dd92c836 11 * http://www.pololu.com/
DrCoyle 0:f186dd92c836 12 * http://forum.pololu.com/
DrCoyle 0:f186dd92c836 13 *
DrCoyle 0:f186dd92c836 14 * Permission is hereby granted, free of charge, to any person
DrCoyle 0:f186dd92c836 15 * obtaining a copy of this software and associated documentation
DrCoyle 0:f186dd92c836 16 * files (the "Software"), to deal in the Software without
DrCoyle 0:f186dd92c836 17 * restriction, including without limitation the rights to use,
DrCoyle 0:f186dd92c836 18 * copy, modify, merge, publish, distribute, sublicense, and/or sell
DrCoyle 0:f186dd92c836 19 * copies of the Software, and to permit persons to whom the
DrCoyle 0:f186dd92c836 20 * Software is furnished to do so, subject to the following
DrCoyle 0:f186dd92c836 21 * conditions:
DrCoyle 0:f186dd92c836 22 *
DrCoyle 0:f186dd92c836 23 * The above copyright notice and this permission notice shall be
DrCoyle 0:f186dd92c836 24 * included in all copies or substantial portions of the Software.
DrCoyle 0:f186dd92c836 25 *
DrCoyle 0:f186dd92c836 26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
DrCoyle 0:f186dd92c836 27 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
DrCoyle 0:f186dd92c836 28 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
DrCoyle 0:f186dd92c836 29 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
DrCoyle 0:f186dd92c836 30 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
DrCoyle 0:f186dd92c836 31 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
DrCoyle 0:f186dd92c836 32 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
DrCoyle 0:f186dd92c836 33 * OTHER DEALINGS IN THE SOFTWARE.
DrCoyle 0:f186dd92c836 34 *
DrCoyle 0:f186dd92c836 35 */
DrCoyle 0:f186dd92c836 36 #include "LSM303D.h"
DrCoyle 0:f186dd92c836 37 #include "mbed.h"
DrCoyle 0:f186dd92c836 38
DrCoyle 0:f186dd92c836 39 #ifndef M_PI
DrCoyle 0:f186dd92c836 40 #define M_PI 3.14159265358979323846
DrCoyle 0:f186dd92c836 41 #endif
DrCoyle 0:f186dd92c836 42
DrCoyle 0:f186dd92c836 43 LSM303D::LSM303D(SPI &spi, PinName CS) : cs(CS), mspi(spi) {
DrCoyle 0:f186dd92c836 44 cs = 1;
DrCoyle 0:f186dd92c836 45 }
DrCoyle 0:f186dd92c836 46
DrCoyle 0:f186dd92c836 47 int LSM303D::whoami() {
DrCoyle 0:f186dd92c836 48 int me;
DrCoyle 0:f186dd92c836 49 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 50
DrCoyle 0:f186dd92c836 51 //Send who am I (first bit must be 1 for read)
DrCoyle 0:f186dd92c836 52 mspi.write(0x8F);
DrCoyle 0:f186dd92c836 53
DrCoyle 0:f186dd92c836 54 //Get who am I response
DrCoyle 0:f186dd92c836 55 me = mspi.write(0x00);
DrCoyle 0:f186dd92c836 56 cs=1; //done talking
DrCoyle 0:f186dd92c836 57
DrCoyle 0:f186dd92c836 58 return me;
DrCoyle 0:f186dd92c836 59 }
DrCoyle 0:f186dd92c836 60
DrCoyle 0:f186dd92c836 61 int LSM303D::initialize() {
DrCoyle 0:f186dd92c836 62 int iam;
DrCoyle 0:f186dd92c836 63
DrCoyle 0:f186dd92c836 64 //Check device
DrCoyle 0:f186dd92c836 65 iam=whoami();
DrCoyle 0:f186dd92c836 66
DrCoyle 0:f186dd92c836 67 if (iam==73) {
DrCoyle 0:f186dd92c836 68
DrCoyle 0:f186dd92c836 69 //set up accelerometer
DrCoyle 0:f186dd92c836 70 //CTRL1
DrCoyle 0:f186dd92c836 71 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 72 mspi.write(0x20);
hassan_elahi 2:171076b97de0 73 mspi.write(0x67); //100Hz, continuous update, all axes enabled
DrCoyle 0:f186dd92c836 74 cs=1; //done talking
DrCoyle 0:f186dd92c836 75
DrCoyle 0:f186dd92c836 76
DrCoyle 0:f186dd92c836 77 //CTRL2
DrCoyle 0:f186dd92c836 78 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 79 mspi.write(0x21);
DrCoyle 0:f186dd92c836 80 mspi.write(0x00); // 773Hz anti-alias filter, +/- 2g scale, self-test disabled, 4-wire SPI
DrCoyle 0:f186dd92c836 81 cs=1; //done talking
DrCoyle 0:f186dd92c836 82
DrCoyle 0:f186dd92c836 83 //CTRL3
DrCoyle 0:f186dd92c836 84
DrCoyle 0:f186dd92c836 85 //set up magnetometer
DrCoyle 0:f186dd92c836 86 //CTRL5
DrCoyle 0:f186dd92c836 87 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 88 mspi.write(0x24);
hassan_elahi 1:3948b63ff4bd 89 mspi.write(0x74); //temperature disabled, high resolution, 100 Hz, int2 disable, int1 disabled
DrCoyle 0:f186dd92c836 90 cs=1; //done talking
DrCoyle 0:f186dd92c836 91
DrCoyle 0:f186dd92c836 92
DrCoyle 0:f186dd92c836 93 //CTRL6 (Magnetic Scale)
DrCoyle 0:f186dd92c836 94 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 95 mspi.write(0x25);
DrCoyle 0:f186dd92c836 96 mspi.write(0x20); //+/-4g
DrCoyle 0:f186dd92c836 97 cs=1; //done talking
DrCoyle 0:f186dd92c836 98
DrCoyle 0:f186dd92c836 99 //CTRL7 (filtering settings, and other)
DrCoyle 0:f186dd92c836 100 cs=0; //talk to compass
DrCoyle 0:f186dd92c836 101 mspi.write(0x26);
DrCoyle 0:f186dd92c836 102 mspi.write(0x00); //normal mode, keep other on
DrCoyle 0:f186dd92c836 103 cs=1; //done talking
DrCoyle 0:f186dd92c836 104
DrCoyle 0:f186dd92c836 105 return 1;
DrCoyle 0:f186dd92c836 106 }
DrCoyle 0:f186dd92c836 107 else {
DrCoyle 0:f186dd92c836 108 //Not talking to right device
DrCoyle 0:f186dd92c836 109 return 0;
DrCoyle 0:f186dd92c836 110 }
DrCoyle 0:f186dd92c836 111 }
DrCoyle 0:f186dd92c836 112
DrCoyle 0:f186dd92c836 113 void LSM303D::setOffset(float x, float y, float z)
DrCoyle 0:f186dd92c836 114 {
DrCoyle 0:f186dd92c836 115 _offset_x = x;
DrCoyle 0:f186dd92c836 116 _offset_y = y;
DrCoyle 0:f186dd92c836 117 _offset_z = z;
DrCoyle 0:f186dd92c836 118 }
DrCoyle 0:f186dd92c836 119
DrCoyle 0:f186dd92c836 120 void LSM303D::setScale(float x, float y, float z)
DrCoyle 0:f186dd92c836 121 {
DrCoyle 0:f186dd92c836 122 _scale_x = x;
DrCoyle 0:f186dd92c836 123 _scale_y = y;
DrCoyle 0:f186dd92c836 124 _scale_z = z;
DrCoyle 0:f186dd92c836 125 }
DrCoyle 0:f186dd92c836 126
DrCoyle 0:f186dd92c836 127 int LSM303D::magnitometer(int axis) {
DrCoyle 0:f186dd92c836 128 if (axis==0) {
DrCoyle 0:f186dd92c836 129 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 130 mspi.write(0x88);
DrCoyle 0:f186dd92c836 131 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 132
DrCoyle 0:f186dd92c836 133 cs=1; //done talking
DrCoyle 0:f186dd92c836 134 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 135
DrCoyle 0:f186dd92c836 136 mspi.write(0x89);
DrCoyle 0:f186dd92c836 137 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 138
DrCoyle 0:f186dd92c836 139 cs=1; //done talking
DrCoyle 0:f186dd92c836 140 }
DrCoyle 0:f186dd92c836 141 else if (axis==1) {
DrCoyle 0:f186dd92c836 142 //Y-Axis
DrCoyle 0:f186dd92c836 143 cs=0;
DrCoyle 0:f186dd92c836 144 mspi.write(0x8A);
DrCoyle 0:f186dd92c836 145 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 146
DrCoyle 0:f186dd92c836 147 cs=1; //done talking
DrCoyle 0:f186dd92c836 148 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 149
DrCoyle 0:f186dd92c836 150 mspi.write(0x8B);
DrCoyle 0:f186dd92c836 151 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 152
DrCoyle 0:f186dd92c836 153 cs=1; //done talking
DrCoyle 0:f186dd92c836 154 }
DrCoyle 0:f186dd92c836 155 else if (axis==2) {
DrCoyle 0:f186dd92c836 156 //Z-Axis
DrCoyle 0:f186dd92c836 157 cs=0;
DrCoyle 0:f186dd92c836 158 mspi.write(0x8C);
DrCoyle 0:f186dd92c836 159 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 160
DrCoyle 0:f186dd92c836 161 cs=1; //done talking
DrCoyle 0:f186dd92c836 162 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 163
DrCoyle 0:f186dd92c836 164 mspi.write(0x8D);
DrCoyle 0:f186dd92c836 165 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 166
DrCoyle 0:f186dd92c836 167 cs=1; //done talking
DrCoyle 0:f186dd92c836 168 }
DrCoyle 0:f186dd92c836 169 return compass.raw;
DrCoyle 0:f186dd92c836 170 }
DrCoyle 0:f186dd92c836 171
DrCoyle 0:f186dd92c836 172 int LSM303D::accelerometer(int axis) {
DrCoyle 0:f186dd92c836 173 if (axis==0) {
DrCoyle 0:f186dd92c836 174 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 175 mspi.write(0xA8);
DrCoyle 0:f186dd92c836 176 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 177
DrCoyle 0:f186dd92c836 178 cs=1; //done talking
DrCoyle 0:f186dd92c836 179 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 180
DrCoyle 0:f186dd92c836 181 mspi.write(0xA9);
DrCoyle 0:f186dd92c836 182 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 183
DrCoyle 0:f186dd92c836 184 cs=1; //done talking
DrCoyle 0:f186dd92c836 185 }
DrCoyle 0:f186dd92c836 186 else if (axis==1) {
DrCoyle 0:f186dd92c836 187 //Y-Axis
DrCoyle 0:f186dd92c836 188 cs=0;
DrCoyle 0:f186dd92c836 189 mspi.write(0xAA);
DrCoyle 0:f186dd92c836 190 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 191
DrCoyle 0:f186dd92c836 192 cs=1; //done talking
DrCoyle 0:f186dd92c836 193 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 194
DrCoyle 0:f186dd92c836 195 mspi.write(0xAB);
DrCoyle 0:f186dd92c836 196 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 197
DrCoyle 0:f186dd92c836 198 cs=1; //done talking
DrCoyle 0:f186dd92c836 199 }
DrCoyle 0:f186dd92c836 200 else if (axis==2) {
DrCoyle 0:f186dd92c836 201 //Z-Axis
DrCoyle 0:f186dd92c836 202 cs=0;
DrCoyle 0:f186dd92c836 203 mspi.write(0xAC);
DrCoyle 0:f186dd92c836 204 compass.b[0] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 205
DrCoyle 0:f186dd92c836 206 cs=1; //done talking
DrCoyle 0:f186dd92c836 207 cs=0; //lower cs to talk
DrCoyle 0:f186dd92c836 208
DrCoyle 0:f186dd92c836 209 mspi.write(0xAD);
DrCoyle 0:f186dd92c836 210 compass.b[1] = mspi.write(0x00);
DrCoyle 0:f186dd92c836 211
DrCoyle 0:f186dd92c836 212 cs=1; //done talking
DrCoyle 0:f186dd92c836 213 }
DrCoyle 0:f186dd92c836 214 return compass.raw;
DrCoyle 0:f186dd92c836 215 }
DrCoyle 0:f186dd92c836 216
DrCoyle 0:f186dd92c836 217 void LSM303D::read(vector &a, vector &m)
DrCoyle 0:f186dd92c836 218 {
DrCoyle 0:f186dd92c836 219 short a_x, a_y, a_z;
DrCoyle 0:f186dd92c836 220 short m_x, m_y, m_z;
DrCoyle 0:f186dd92c836 221 //Timer t;
DrCoyle 0:f186dd92c836 222 //int usec1, usec2;
DrCoyle 0:f186dd92c836 223
DrCoyle 0:f186dd92c836 224 //t.reset();
DrCoyle 0:f186dd92c836 225 //t.start();
DrCoyle 0:f186dd92c836 226
DrCoyle 0:f186dd92c836 227 //usec1 = t.read_us();
DrCoyle 0:f186dd92c836 228 /*read_reg_short(addr_acc, OUT_X_A, &a_x);
DrCoyle 0:f186dd92c836 229 read_reg_short(addr_acc, OUT_Y_A, &a_y);
DrCoyle 0:f186dd92c836 230 read_reg_short(addr_acc, OUT_Z_A, &a_z);
DrCoyle 0:f186dd92c836 231 read_reg_short(addr_mag, OUT_X_M, &m_x);
DrCoyle 0:f186dd92c836 232 read_reg_short(addr_mag, OUT_Y_M, &m_y);
DrCoyle 0:f186dd92c836 233 read_reg_short(addr_mag, OUT_Z_M, &m_z);*/
DrCoyle 0:f186dd92c836 234
DrCoyle 0:f186dd92c836 235 a_x=accelerometer(XAXIS);
DrCoyle 0:f186dd92c836 236 a_y=accelerometer(YAXIS);
DrCoyle 0:f186dd92c836 237 a_z=accelerometer(ZAXIS);
DrCoyle 0:f186dd92c836 238 m_x=magnitometer(XAXIS);
DrCoyle 0:f186dd92c836 239 m_y=magnitometer(YAXIS);
DrCoyle 0:f186dd92c836 240 m_z=magnitometer(ZAXIS);
DrCoyle 0:f186dd92c836 241
DrCoyle 0:f186dd92c836 242 //usec2 = t.read_us();
DrCoyle 0:f186dd92c836 243
DrCoyle 0:f186dd92c836 244 //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
DrCoyle 0:f186dd92c836 245
DrCoyle 0:f186dd92c836 246 // Perform simple lowpass filtering
DrCoyle 0:f186dd92c836 247 // Intended to stabilize heading despite
DrCoyle 0:f186dd92c836 248 // device vibration such as on a UGV
DrCoyle 0:f186dd92c836 249 _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 250 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 251 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 252
DrCoyle 0:f186dd92c836 253 a.x = (float) (_filt_ax >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 254 a.y = (float) (_filt_ay >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 255 a.z = (float) (_filt_az >> FILTER_SHIFT);
DrCoyle 0:f186dd92c836 256
DrCoyle 0:f186dd92c836 257 // offset and scale
DrCoyle 0:f186dd92c836 258 m.x = (m_x + _offset_x) * _scale_x;
DrCoyle 0:f186dd92c836 259 m.y = (m_y + _offset_y) * _scale_y;
DrCoyle 0:f186dd92c836 260 m.z = (m_z + _offset_z) * _scale_z;
DrCoyle 0:f186dd92c836 261 }
DrCoyle 0:f186dd92c836 262
DrCoyle 0:f186dd92c836 263 /// Returns the number of degrees from the -Y axis that it
DrCoyle 0:f186dd92c836 264 // is pointing.
DrCoyle 0:f186dd92c836 265 float LSM303D::heading()
DrCoyle 0:f186dd92c836 266 {
DrCoyle 0:f186dd92c836 267 return heading((vector){0,-1,0});
DrCoyle 0:f186dd92c836 268 }
DrCoyle 0:f186dd92c836 269
DrCoyle 0:f186dd92c836 270 float LSM303D::heading(vector from)
DrCoyle 0:f186dd92c836 271 {
DrCoyle 0:f186dd92c836 272 vector a, m;
DrCoyle 0:f186dd92c836 273
DrCoyle 0:f186dd92c836 274 this->read(a, m);
DrCoyle 0:f186dd92c836 275
DrCoyle 0:f186dd92c836 276 ////////////////////////////////////////////////
DrCoyle 0:f186dd92c836 277 // compute heading
DrCoyle 0:f186dd92c836 278 ////////////////////////////////////////////////
DrCoyle 0:f186dd92c836 279
DrCoyle 0:f186dd92c836 280 vector temp_a = a;
DrCoyle 0:f186dd92c836 281 // normalize
DrCoyle 0:f186dd92c836 282 vector_normalize(&temp_a);
DrCoyle 0:f186dd92c836 283 //vector_normalize(&m);
DrCoyle 0:f186dd92c836 284
DrCoyle 0:f186dd92c836 285 // compute E and N
DrCoyle 0:f186dd92c836 286 vector E;
DrCoyle 0:f186dd92c836 287 vector N;
DrCoyle 0:f186dd92c836 288 vector_cross(&m,&temp_a,&E);
DrCoyle 0:f186dd92c836 289 vector_normalize(&E);
DrCoyle 0:f186dd92c836 290 vector_cross(&temp_a,&E,&N);
DrCoyle 0:f186dd92c836 291
DrCoyle 0:f186dd92c836 292 // compute heading
DrCoyle 0:f186dd92c836 293 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
DrCoyle 0:f186dd92c836 294 if (heading < 0) heading += 360;
DrCoyle 0:f186dd92c836 295
DrCoyle 0:f186dd92c836 296 return heading;
DrCoyle 0:f186dd92c836 297 }