Alexandre Salconi-Denis / LSM303DLH

My attempt to made a more useful lib. You can get the accelerator and magnetometer.

Fork of LSM303DLH by Michael Shimniok

LSM303DLH.cpp@8:cc338ded0b2c, 2017-08-06 (annotated)

Committer:
salco
Date:
Sun Aug 06 23:57:25 2017 +0000
Revision:
8:cc338ded0b2c
Parent:
7:275a0a69cff5 
remove part of my debug comment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:de767f4959ef 1 /** LSM303DLH Interface Library
shimniok 0:de767f4959ef 2 *
shimniok 0:de767f4959ef 3 * Michael Shimniok http://bot-thoughts.com
salco 7:275a0a69cff5 4 * Modified by @author Salco <JeSuisSalco@gmail.com>
shimniok 0:de767f4959ef 5 *
salco 7:275a0a69cff5 6 * This file is subject to the terms and conditions of the GNU Lesser
salco 7:275a0a69cff5 7 * General Public License v2.1. See the file LICENSE in the top level
salco 7:275a0a69cff5 8 * directory for more details.
shimniok 0:de767f4959ef 9 */
shimniok 0:de767f4959ef 10 #include "mbed.h"
shimniok 0:de767f4959ef 11 #include "LSM303DLH.h"
shimniok 0:de767f4959ef 12
shimniok 0:de767f4959ef 13 #ifndef M_PI
shimniok 0:de767f4959ef 14 #define M_PI 3.14159265358979323846
shimniok 0:de767f4959ef 15 #endif
shimniok 0:de767f4959ef 16
shimniok 1:48d83c63d1d9 17 #define FILTER_SHIFT 6 // used in filtering acceleromter readings
shimniok 1:48d83c63d1d9 18
salco 6:86cf2afe3e52 19
shimniok 0:de767f4959ef 20
salco 5:48722ae56546 21
salco 5:48722ae56546 22
salco 5:48722ae56546 23 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, uint8_t v)
salco 5:48722ae56546 24 {
salco 5:48722ae56546 25 return this->write_reg(addr_i2c,addr_reg,(char)v);
salco 5:48722ae56546 26 }
shimniok 0:de767f4959ef 27
shimniok 0:de767f4959ef 28 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
shimniok 0:de767f4959ef 29 {
salco 3:9b4ff901b5c9 30 bool result=false;
salco 3:9b4ff901b5c9 31 char data[2] = {addr_reg, v};
salco 3:9b4ff901b5c9 32 //__disable_irq();
salco 3:9b4ff901b5c9 33 result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
salco 3:9b4ff901b5c9 34 if(result == false) debug("Unable to Write \n");
salco 3:9b4ff901b5c9 35
salco 3:9b4ff901b5c9 36 //__enable_irq();
salco 3:9b4ff901b5c9 37 return result;
shimniok 0:de767f4959ef 38 }
shimniok 0:de767f4959ef 39
salco 5:48722ae56546 40 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, uint8_t *v)
salco 5:48722ae56546 41 {
salco 5:48722ae56546 42 return this->read_reg(addr_i2c,addr_reg,(char*)v);
salco 5:48722ae56546 43 }
salco 5:48722ae56546 44
salco 5:48722ae56546 45
shimniok 0:de767f4959ef 46 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
shimniok 0:de767f4959ef 47 {
shimniok 0:de767f4959ef 48 char data = addr_reg;
shimniok 2:aea5caec809c 49 bool result = false;
shimniok 2:aea5caec809c 50
salco 3:9b4ff901b5c9 51 //__disable_irq();
salco 3:9b4ff901b5c9 52 if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 53 {
salco 3:9b4ff901b5c9 54 if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 55 {
salco 3:9b4ff901b5c9 56 *v = data;
salco 3:9b4ff901b5c9 57 result = true;
salco 3:9b4ff901b5c9 58 }
salco 3:9b4ff901b5c9 59 else
salco 3:9b4ff901b5c9 60 {
salco 3:9b4ff901b5c9 61 debug("Unable to Read \n");
salco 3:9b4ff901b5c9 62 }
shimniok 0:de767f4959ef 63 }
salco 3:9b4ff901b5c9 64 else
salco 3:9b4ff901b5c9 65 {
salco 3:9b4ff901b5c9 66 debug("Unable to Write \n");
salco 3:9b4ff901b5c9 67 }
salco 3:9b4ff901b5c9 68 //__enable_irq();
shimniok 2:aea5caec809c 69 return result;
shimniok 0:de767f4959ef 70 }
shimniok 0:de767f4959ef 71
shimniok 0:de767f4959ef 72 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
shimniok 0:de767f4959ef 73 {
salco 3:9b4ff901b5c9 74
shimniok 0:de767f4959ef 75 char *pv = (char *)v;
shimniok 2:aea5caec809c 76 bool result;
shimniok 2:aea5caec809c 77
shimniok 2:aea5caec809c 78 result = read_reg(addr_i2c,addr_reg+0,pv+1);
shimniok 2:aea5caec809c 79 result &= read_reg(addr_i2c,addr_reg+1,pv+0);
shimniok 2:aea5caec809c 80
salco 3:9b4ff901b5c9 81
shimniok 2:aea5caec809c 82 return result;
shimniok 0:de767f4959ef 83 }
shimniok 0:de767f4959ef 84
salco 6:86cf2afe3e52 85 bool LSM303DLH::read_reg_short(DEV_ADDRS addr_i2c,REG_ADDRS addr_reg, OUT_XYZ_t *dataRead)
salco 6:86cf2afe3e52 86 {
salco 6:86cf2afe3e52 87 bool result=true;
salco 6:86cf2afe3e52 88
salco 6:86cf2afe3e52 89 switch(addr_reg)
salco 6:86cf2afe3e52 90 {
salco 6:86cf2afe3e52 91 case OUT_X_A:
salco 6:86cf2afe3e52 92 case OUT_Y_A:
salco 6:86cf2afe3e52 93 case OUT_Z_A:
salco 6:86cf2afe3e52 94
salco 6:86cf2afe3e52 95 case OUT_X_M:
salco 6:86cf2afe3e52 96 case OUT_Y_M:
salco 6:86cf2afe3e52 97 case OUT_Z_M:
salco 6:86cf2afe3e52 98 result &= read_reg(addr_i2c,addr_reg ,&(dataRead->UT_L_A));//LSB at lower
salco 6:86cf2afe3e52 99 result &= read_reg(addr_i2c,addr_reg+1,&(dataRead->UT_H_A));
salco 6:86cf2afe3e52 100 #if defined(LSM303_LITLE_ENDIAN)
salco 6:86cf2afe3e52 101 dataRead.value = (dataRead.byte[1]<<8)+(dataRead.byte[0]); //swap the reading
salco 6:86cf2afe3e52 102 #endif
salco 6:86cf2afe3e52 103 break;
salco 6:86cf2afe3e52 104 default:
salco 6:86cf2afe3e52 105 result = false;
salco 6:86cf2afe3e52 106 break;
salco 6:86cf2afe3e52 107 }
salco 6:86cf2afe3e52 108
salco 6:86cf2afe3e52 109 return result;
salco 6:86cf2afe3e52 110 }
salco 6:86cf2afe3e52 111
salco 6:86cf2afe3e52 112 LSM303DLH::~LSM303DLH()
salco 6:86cf2afe3e52 113 {
salco 6:86cf2afe3e52 114 if(m_have_createdI2C)
salco 6:86cf2afe3e52 115 {
salco 6:86cf2afe3e52 116 delete m_ptr_I2C;
salco 6:86cf2afe3e52 117 }
salco 6:86cf2afe3e52 118 }
salco 3:9b4ff901b5c9 119 LSM303DLH::LSM303DLH(PinName sda, PinName scl)
salco 3:9b4ff901b5c9 120 {
salco 3:9b4ff901b5c9 121 m_ptr_I2C = new I2C(sda, scl);
salco 6:86cf2afe3e52 122 m_have_createdI2C = true;
salco 6:86cf2afe3e52 123 this->init();
salco 3:9b4ff901b5c9 124 }
salco 3:9b4ff901b5c9 125 LSM303DLH::LSM303DLH(I2C* ptrI2C)
salco 3:9b4ff901b5c9 126 {
salco 3:9b4ff901b5c9 127 m_ptr_I2C = ptrI2C;
salco 6:86cf2afe3e52 128 m_have_createdI2C = false;
salco 3:9b4ff901b5c9 129 this->init();
salco 3:9b4ff901b5c9 130 }
salco 3:9b4ff901b5c9 131
salco 3:9b4ff901b5c9 132 void LSM303DLH::init(void)
shimniok 0:de767f4959ef 133 {
shimniok 0:de767f4959ef 134 char reg_v;
salco 3:9b4ff901b5c9 135
salco 3:9b4ff901b5c9 136 _offset_x = 0;
salco 3:9b4ff901b5c9 137 _offset_y = 0;
salco 3:9b4ff901b5c9 138 _offset_z = 0;
salco 3:9b4ff901b5c9 139 _scale_x = 0;
salco 3:9b4ff901b5c9 140 _scale_y = 0;
salco 3:9b4ff901b5c9 141 _scale_z = 0;
salco 3:9b4ff901b5c9 142 _filt_ax = 0;
salco 3:9b4ff901b5c9 143 _filt_ay = 0;
salco 3:9b4ff901b5c9 144 _filt_az = 6000;
salco 3:9b4ff901b5c9 145
salco 3:9b4ff901b5c9 146
salco 3:9b4ff901b5c9 147 m_ptr_I2C->frequency(100000);
salco 3:9b4ff901b5c9 148
salco 4:4f2ed3f8726c 149 ((Ctrl_Reg1_A_t*)&reg_v)->byte = 0;
salco 4:4f2ed3f8726c 150 ((Ctrl_Reg1_A_t*)&reg_v)->ODR |= 0b0010; /* Normal mode */
salco 4:4f2ed3f8726c 151 ((Ctrl_Reg1_A_t*)&reg_v)->Xen |= 1; /* X/Y/Z axis enable. */
salco 4:4f2ed3f8726c 152 ((Ctrl_Reg1_A_t*)&reg_v)->Yen |= 1;
salco 4:4f2ed3f8726c 153 ((Ctrl_Reg1_A_t*)&reg_v)->Zen |= 1;
shimniok 0:de767f4959ef 154 write_reg(addr_acc,CTRL_REG1_A,reg_v);
salco 4:4f2ed3f8726c 155 //not sure if we need to read the register
salco 4:4f2ed3f8726c 156 //reg_v = 0;
salco 4:4f2ed3f8726c 157 //read_reg(addr_acc,CTRL_REG1_A,&reg_v);
shimniok 0:de767f4959ef 158
salco 6:86cf2afe3e52 159 ((Ctrl_Reg4_A_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 160 ((Ctrl_Reg4_A_t*)&reg_v)->BDU |= 1; //full read befor update
salco 6:86cf2afe3e52 161 //(((Ctrl_Reg4_A_t*)&reg_v)->HR) |= 1; //hi res
salco 6:86cf2afe3e52 162 #if defined(LSM303_LITLE_ENDIAN)
salco 6:86cf2afe3e52 163 ((Ctrl_Reg4_A_t*)&reg_v)->BLE |= 1; /* 1: data MSB @ lower address */
salco 6:86cf2afe3e52 164 #endif
salco 6:86cf2afe3e52 165 ((Ctrl_Reg4_A_t*)&reg_v)->FS |= 0b01; ; /* +/- 4g */
shimniok 0:de767f4959ef 166 write_reg(addr_acc,CTRL_REG4_A,reg_v);
shimniok 0:de767f4959ef 167
shimniok 0:de767f4959ef 168 /* -- mag --- */
salco 3:9b4ff901b5c9 169 debug("in MAG \n");
salco 6:86cf2afe3e52 170 ((CRA_REG_M_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 171 ((CRA_REG_M_t*)&reg_v)->DO |= 0b100; /* Minimum data output rate = 15Hz */
shimniok 0:de767f4959ef 172 write_reg(addr_mag,CRA_REG_M,reg_v);
shimniok 0:de767f4959ef 173
shimniok 0:de767f4959ef 174 reg_v = 0;
shimniok 0:de767f4959ef 175 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
salco 6:86cf2afe3e52 176 ((CRB_REG_M_t*)&reg_v)->GN |= 0b111; /* +-8.1Gauss */
shimniok 0:de767f4959ef 177 write_reg(addr_mag,CRB_REG_M,reg_v);
shimniok 0:de767f4959ef 178
salco 6:86cf2afe3e52 179 ((MR_REG_M_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 180 //((MR_REG_M_t*)&reg_v)->MD |= 0; /* Continuous-conversion mode */
shimniok 0:de767f4959ef 181 write_reg(addr_mag,MR_REG_M,reg_v);
salco 3:9b4ff901b5c9 182
salco 6:86cf2afe3e52 183 //put here since we dont change it during the execution
salco 6:86cf2afe3e52 184 m_FS = get_FullScall_selection();
salco 6:86cf2afe3e52 185
salco 6:86cf2afe3e52 186 read_reg(addr_mag,CRB_REG_M ,&reg_v);
salco 6:86cf2afe3e52 187 m_GN = (((CRB_REG_M_t*)&reg_v)->GN)-1;
shimniok 0:de767f4959ef 188 }
shimniok 0:de767f4959ef 189
shimniok 0:de767f4959ef 190 void LSM303DLH::setOffset(float x, float y, float z)
shimniok 0:de767f4959ef 191 {
shimniok 0:de767f4959ef 192 _offset_x = x;
shimniok 0:de767f4959ef 193 _offset_y = y;
shimniok 0:de767f4959ef 194 _offset_z = z;
shimniok 0:de767f4959ef 195 }
shimniok 0:de767f4959ef 196
shimniok 0:de767f4959ef 197 void LSM303DLH::setScale(float x, float y, float z)
shimniok 0:de767f4959ef 198 {
shimniok 0:de767f4959ef 199 _scale_x = x;
shimniok 0:de767f4959ef 200 _scale_y = y;
shimniok 0:de767f4959ef 201 _scale_z = z;
shimniok 0:de767f4959ef 202 }
salco 5:48722ae56546 203 //#define _FS 4
salco 3:9b4ff901b5c9 204 bool LSM303DLH::read(vector &a, vector &m)
shimniok 0:de767f4959ef 205 {
salco 3:9b4ff901b5c9 206
salco 3:9b4ff901b5c9 207 bool result = true;
salco 5:48722ae56546 208 //short a_x, a_y, a_z;
salco 6:86cf2afe3e52 209 //short m_x, m_y, m_z;
salco 3:9b4ff901b5c9 210 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 211 Timer t;
salco 3:9b4ff901b5c9 212 int usec1, usec2;
salco 3:9b4ff901b5c9 213
salco 3:9b4ff901b5c9 214 t.reset();
salco 3:9b4ff901b5c9 215 t.start();
shimniok 2:aea5caec809c 216
salco 3:9b4ff901b5c9 217 usec1 = t.read_us();
salco 3:9b4ff901b5c9 218 #endif
salco 3:9b4ff901b5c9 219
salco 8:cc338ded0b2c 220 static vector local_a, local_m;
shimniok 0:de767f4959ef 221
salco 8:cc338ded0b2c 222 result &= read_acc_raw(&local_a);
salco 5:48722ae56546 223
salco 8:cc338ded0b2c 224 result &= read_mag_raw(&local_m);
salco 6:86cf2afe3e52 225
salco 6:86cf2afe3e52 226
salco 3:9b4ff901b5c9 227 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 228 usec2 = t.read_us();
salco 3:9b4ff901b5c9 229
salco 3:9b4ff901b5c9 230 debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
salco 3:9b4ff901b5c9 231 #endif
salco 3:9b4ff901b5c9 232 if(result == true)
salco 3:9b4ff901b5c9 233 {
salco 3:9b4ff901b5c9 234 // Perform simple lowpass filtering
salco 3:9b4ff901b5c9 235 // Intended to stabilize heading despite
salco 3:9b4ff901b5c9 236 // device vibration such as on a UGV
salco 3:9b4ff901b5c9 237
salco 6:86cf2afe3e52 238
salco 3:9b4ff901b5c9 239 //float( a[i] ) * pow(2.,(fs+1)) / 32768.
salco 3:9b4ff901b5c9 240
salco 3:9b4ff901b5c9 241 //x/8 = reading /0xFFFF(655355)
salco 3:9b4ff901b5c9 242
salco 3:9b4ff901b5c9 243
salco 3:9b4ff901b5c9 244
salco 3:9b4ff901b5c9 245 // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
salco 5:48722ae56546 246 /* _filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
salco 3:9b4ff901b5c9 247 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 248 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 249
salco 3:9b4ff901b5c9 250
salco 3:9b4ff901b5c9 251
salco 3:9b4ff901b5c9 252 a.x = (float) (_filt_ax >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 253 a.y = (float) (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 254 a.z = (float) (_filt_az >> FILTER_SHIFT);*/
salco 6:86cf2afe3e52 255
salco 8:cc338ded0b2c 256 a = local_a;
salco 6:86cf2afe3e52 257
salco 6:86cf2afe3e52 258
salco 6:86cf2afe3e52 259 // offset and scale
salco 8:cc338ded0b2c 260 m.x = (/*m_*/local_m.x + _offset_x) * _scale_x;
salco 8:cc338ded0b2c 261 m.y = (/*m_*/local_m.y + _offset_y) * _scale_y;
salco 8:cc338ded0b2c 262 m.z = (/*m_*/local_m.z + _offset_z) * _scale_z;
salco 6:86cf2afe3e52 263
salco 3:9b4ff901b5c9 264 }
salco 3:9b4ff901b5c9 265
salco 3:9b4ff901b5c9 266 return result;
shimniok 0:de767f4959ef 267 }
shimniok 0:de767f4959ef 268
shimniok 0:de767f4959ef 269
shimniok 0:de767f4959ef 270 // Returns the number of degrees from the -Y axis that it
shimniok 0:de767f4959ef 271 // is pointing.
shimniok 0:de767f4959ef 272 float LSM303DLH::heading()
shimniok 0:de767f4959ef 273 {
shimniok 0:de767f4959ef 274 return heading((vector){0,-1,0});
shimniok 0:de767f4959ef 275 }
shimniok 0:de767f4959ef 276
shimniok 0:de767f4959ef 277 float LSM303DLH::heading(vector from)
shimniok 0:de767f4959ef 278 {
shimniok 0:de767f4959ef 279 vector a, m;
shimniok 0:de767f4959ef 280
salco 3:9b4ff901b5c9 281 read(a, m);
shimniok 0:de767f4959ef 282
shimniok 0:de767f4959ef 283 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 284 // compute heading
shimniok 0:de767f4959ef 285 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 286
shimniok 0:de767f4959ef 287 vector temp_a = a;
shimniok 0:de767f4959ef 288 // normalize
shimniok 0:de767f4959ef 289 vector_normalize(&temp_a);
shimniok 0:de767f4959ef 290 //vector_normalize(&m);
shimniok 0:de767f4959ef 291
shimniok 0:de767f4959ef 292 // compute E and N
shimniok 0:de767f4959ef 293 vector E;
shimniok 0:de767f4959ef 294 vector N;
shimniok 0:de767f4959ef 295 vector_cross(&m,&temp_a,&E);
shimniok 0:de767f4959ef 296 vector_normalize(&E);
shimniok 0:de767f4959ef 297 vector_cross(&temp_a,&E,&N);
shimniok 0:de767f4959ef 298
shimniok 0:de767f4959ef 299 // compute heading
shimniok 0:de767f4959ef 300 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
shimniok 0:de767f4959ef 301 if (heading < 0) heading += 360;
shimniok 0:de767f4959ef 302
shimniok 0:de767f4959ef 303 return heading;
shimniok 0:de767f4959ef 304 }
shimniok 2:aea5caec809c 305
shimniok 2:aea5caec809c 306 void LSM303DLH::frequency(int hz)
shimniok 2:aea5caec809c 307 {
salco 3:9b4ff901b5c9 308 m_ptr_I2C->frequency(hz);
salco 3:9b4ff901b5c9 309 }
salco 5:48722ae56546 310
salco 5:48722ae56546 311 int8_t LSM303DLH::get_FullScall_selection(void)
salco 5:48722ae56546 312 {
salco 5:48722ae56546 313 char data_read_acc =0;
salco 5:48722ae56546 314 read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
salco 5:48722ae56546 315
salco 5:48722ae56546 316 return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
salco 5:48722ae56546 317 }
salco 6:86cf2afe3e52 318
salco 6:86cf2afe3e52 319 float LSM303DLH::get_acc_value_in_g(OUT_XYZ_t* dataOut)
salco 6:86cf2afe3e52 320 {
salco 6:86cf2afe3e52 321 return (float) (dataOut->value / (float)(32768 /*half of the ADC resolution*/ / m_FS/*+- 4g*/));
salco 6:86cf2afe3e52 322 }
salco 6:86cf2afe3e52 323
salco 6:86cf2afe3e52 324 bool LSM303DLH::read_acc_raw(vector *a)
salco 6:86cf2afe3e52 325 {
salco 6:86cf2afe3e52 326 bool result = true;
salco 6:86cf2afe3e52 327 char data_read_acc =0;
salco 6:86cf2afe3e52 328 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 329
salco 6:86cf2afe3e52 330 read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
salco 6:86cf2afe3e52 331
salco 6:86cf2afe3e52 332 if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
salco 6:86cf2afe3e52 333 {
salco 6:86cf2afe3e52 334 result &= read_reg_short(addr_acc,OUT_X_A,&dataOut);
salco 6:86cf2afe3e52 335
salco 6:86cf2afe3e52 336 if(result)
salco 6:86cf2afe3e52 337 {
salco 6:86cf2afe3e52 338 a->x = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 339 }
salco 6:86cf2afe3e52 340 else
salco 6:86cf2afe3e52 341 {
salco 6:86cf2afe3e52 342 debug("error reading \n");
salco 6:86cf2afe3e52 343 }
salco 6:86cf2afe3e52 344
salco 6:86cf2afe3e52 345 if(result)
salco 6:86cf2afe3e52 346 {
salco 6:86cf2afe3e52 347 result &= read_reg_short(addr_acc,OUT_Y_A,&dataOut);
salco 6:86cf2afe3e52 348 }
salco 6:86cf2afe3e52 349 if(result)
salco 6:86cf2afe3e52 350 {
salco 6:86cf2afe3e52 351 a->y = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 352 }
salco 6:86cf2afe3e52 353 else
salco 6:86cf2afe3e52 354 {
salco 6:86cf2afe3e52 355 debug("error reading \n");
salco 6:86cf2afe3e52 356 }
salco 6:86cf2afe3e52 357
salco 6:86cf2afe3e52 358 if(result)
salco 6:86cf2afe3e52 359 {
salco 6:86cf2afe3e52 360 result &= read_reg_short(addr_acc,OUT_Z_A,&dataOut);
salco 6:86cf2afe3e52 361 }
salco 6:86cf2afe3e52 362 if(result)
salco 6:86cf2afe3e52 363 {
salco 6:86cf2afe3e52 364 a->z = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 365 }
salco 6:86cf2afe3e52 366 else
salco 6:86cf2afe3e52 367 {
salco 6:86cf2afe3e52 368 debug("error reading \n");
salco 6:86cf2afe3e52 369 }
salco 6:86cf2afe3e52 370 }
salco 6:86cf2afe3e52 371
salco 6:86cf2afe3e52 372 return result;
salco 6:86cf2afe3e52 373 }
salco 6:86cf2afe3e52 374 bool LSM303DLH::read_mag_raw(vector *m)
salco 6:86cf2afe3e52 375 {
salco 6:86cf2afe3e52 376 bool result = true;
salco 6:86cf2afe3e52 377 char data_read_mag =0;
salco 6:86cf2afe3e52 378 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 379
salco 6:86cf2afe3e52 380 read_reg(addr_mag,SR_REG_M,&data_read_mag);
salco 6:86cf2afe3e52 381
salco 6:86cf2afe3e52 382
salco 6:86cf2afe3e52 383 /**@todo not sure if the reading of magnetometer is Litle or big endian, I assume its change like the accelerometer.
salco 6:86cf2afe3e52 384 * You can try to find the answer if you care.
salco 6:86cf2afe3e52 385 */
salco 6:86cf2afe3e52 386
salco 6:86cf2afe3e52 387 if(((SR_Reg_M_t*)&data_read_mag)->DRDY)
salco 6:86cf2afe3e52 388 {
salco 6:86cf2afe3e52 389 float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
salco 6:86cf2afe3e52 390 float gainz[] = { 980., 760., 600., 400., 355., 295., 205. };
salco 6:86cf2afe3e52 391
salco 6:86cf2afe3e52 392 result &= read_reg_short(addr_mag,OUT_X_M,&dataOut);
salco 6:86cf2afe3e52 393 if(result)
salco 6:86cf2afe3e52 394 {
salco 8:cc338ded0b2c 395 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 396 m->x = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 397 }
salco 6:86cf2afe3e52 398
salco 6:86cf2afe3e52 399 result &= read_reg_short(addr_mag,OUT_Y_M,&dataOut);
salco 6:86cf2afe3e52 400 if(result)
salco 6:86cf2afe3e52 401 {
salco 8:cc338ded0b2c 402 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 403 m->y = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 404 }
salco 6:86cf2afe3e52 405
salco 6:86cf2afe3e52 406 result &= read_reg_short(addr_mag,OUT_Z_M,&dataOut);
salco 6:86cf2afe3e52 407 if(result)
salco 6:86cf2afe3e52 408 {
salco 8:cc338ded0b2c 409 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 410 m->z = float(dataOut.value) / gainz[m_GN];
salco 6:86cf2afe3e52 411 }
salco 6:86cf2afe3e52 412 }
salco 6:86cf2afe3e52 413
salco 6:86cf2afe3e52 414 return result;
salco 6:86cf2afe3e52 415 }