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@7:275a0a69cff5, 2017-08-06 (annotated)

Committer:
salco
Date:
Sun Aug 06 23:42:07 2017 +0000
Revision:
7:275a0a69cff5
Parent:
6:86cf2afe3e52
Child:
8:cc338ded0b2c
Finished the correction

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 6:86cf2afe3e52 220 //OUT_XYZ_t my_test,dataOut;
salco 6:86cf2afe3e52 221 static vector a_test, m_test;
shimniok 0:de767f4959ef 222
salco 6:86cf2afe3e52 223 result &= read_acc_raw(&a_test);
salco 5:48722ae56546 224
salco 6:86cf2afe3e52 225 result &= read_mag_raw(&m_test);
salco 6:86cf2afe3e52 226
salco 6:86cf2afe3e52 227
salco 3:9b4ff901b5c9 228 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 229 usec2 = t.read_us();
salco 3:9b4ff901b5c9 230
salco 3:9b4ff901b5c9 231 debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
salco 3:9b4ff901b5c9 232 #endif
salco 3:9b4ff901b5c9 233 if(result == true)
salco 3:9b4ff901b5c9 234 {
salco 3:9b4ff901b5c9 235 // Perform simple lowpass filtering
salco 3:9b4ff901b5c9 236 // Intended to stabilize heading despite
salco 3:9b4ff901b5c9 237 // device vibration such as on a UGV
salco 3:9b4ff901b5c9 238
salco 6:86cf2afe3e52 239
salco 3:9b4ff901b5c9 240 //float( a[i] ) * pow(2.,(fs+1)) / 32768.
salco 3:9b4ff901b5c9 241
salco 3:9b4ff901b5c9 242 //x/8 = reading /0xFFFF(655355)
salco 3:9b4ff901b5c9 243
salco 3:9b4ff901b5c9 244
salco 3:9b4ff901b5c9 245
salco 3:9b4ff901b5c9 246 // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
salco 5:48722ae56546 247 /* _filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
salco 3:9b4ff901b5c9 248 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 249 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 250
salco 3:9b4ff901b5c9 251
salco 3:9b4ff901b5c9 252
salco 3:9b4ff901b5c9 253 a.x = (float) (_filt_ax >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 254 a.y = (float) (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 255 a.z = (float) (_filt_az >> FILTER_SHIFT);*/
salco 6:86cf2afe3e52 256
salco 6:86cf2afe3e52 257 a = a_test;
salco 6:86cf2afe3e52 258
salco 6:86cf2afe3e52 259
salco 6:86cf2afe3e52 260 // offset and scale
salco 6:86cf2afe3e52 261 m.x = (/*m_*/m_test.x + _offset_x) * _scale_x;
salco 6:86cf2afe3e52 262 m.y = (/*m_*/m_test.y + _offset_y) * _scale_y;
salco 6:86cf2afe3e52 263 m.z = (/*m_*/m_test.z + _offset_z) * _scale_z;
salco 6:86cf2afe3e52 264
salco 3:9b4ff901b5c9 265 }
salco 3:9b4ff901b5c9 266
salco 3:9b4ff901b5c9 267 return result;
shimniok 0:de767f4959ef 268 }
shimniok 0:de767f4959ef 269
shimniok 0:de767f4959ef 270
shimniok 0:de767f4959ef 271 // Returns the number of degrees from the -Y axis that it
shimniok 0:de767f4959ef 272 // is pointing.
shimniok 0:de767f4959ef 273 float LSM303DLH::heading()
shimniok 0:de767f4959ef 274 {
shimniok 0:de767f4959ef 275 return heading((vector){0,-1,0});
shimniok 0:de767f4959ef 276 }
shimniok 0:de767f4959ef 277
shimniok 0:de767f4959ef 278 float LSM303DLH::heading(vector from)
shimniok 0:de767f4959ef 279 {
shimniok 0:de767f4959ef 280 vector a, m;
shimniok 0:de767f4959ef 281
salco 3:9b4ff901b5c9 282 read(a, m);
shimniok 0:de767f4959ef 283
shimniok 0:de767f4959ef 284 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 285 // compute heading
shimniok 0:de767f4959ef 286 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 287
shimniok 0:de767f4959ef 288 vector temp_a = a;
shimniok 0:de767f4959ef 289 // normalize
shimniok 0:de767f4959ef 290 vector_normalize(&temp_a);
shimniok 0:de767f4959ef 291 //vector_normalize(&m);
shimniok 0:de767f4959ef 292
shimniok 0:de767f4959ef 293 // compute E and N
shimniok 0:de767f4959ef 294 vector E;
shimniok 0:de767f4959ef 295 vector N;
shimniok 0:de767f4959ef 296 vector_cross(&m,&temp_a,&E);
shimniok 0:de767f4959ef 297 vector_normalize(&E);
shimniok 0:de767f4959ef 298 vector_cross(&temp_a,&E,&N);
shimniok 0:de767f4959ef 299
shimniok 0:de767f4959ef 300 // compute heading
shimniok 0:de767f4959ef 301 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
shimniok 0:de767f4959ef 302 if (heading < 0) heading += 360;
shimniok 0:de767f4959ef 303
shimniok 0:de767f4959ef 304 return heading;
shimniok 0:de767f4959ef 305 }
shimniok 2:aea5caec809c 306
shimniok 2:aea5caec809c 307 void LSM303DLH::frequency(int hz)
shimniok 2:aea5caec809c 308 {
salco 3:9b4ff901b5c9 309 m_ptr_I2C->frequency(hz);
salco 3:9b4ff901b5c9 310 }
salco 5:48722ae56546 311
salco 5:48722ae56546 312 int8_t LSM303DLH::get_FullScall_selection(void)
salco 5:48722ae56546 313 {
salco 5:48722ae56546 314 char data_read_acc =0;
salco 5:48722ae56546 315 read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
salco 5:48722ae56546 316
salco 5:48722ae56546 317 return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
salco 5:48722ae56546 318 }
salco 6:86cf2afe3e52 319
salco 6:86cf2afe3e52 320 float LSM303DLH::get_acc_value_in_g(OUT_XYZ_t* dataOut)
salco 6:86cf2afe3e52 321 {
salco 6:86cf2afe3e52 322 return (float) (dataOut->value / (float)(32768 /*half of the ADC resolution*/ / m_FS/*+- 4g*/));
salco 6:86cf2afe3e52 323 }
salco 6:86cf2afe3e52 324
salco 6:86cf2afe3e52 325 bool LSM303DLH::read_acc_raw(vector *a)
salco 6:86cf2afe3e52 326 {
salco 6:86cf2afe3e52 327 bool result = true;
salco 6:86cf2afe3e52 328 char data_read_acc =0;
salco 6:86cf2afe3e52 329 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 330
salco 6:86cf2afe3e52 331 read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
salco 6:86cf2afe3e52 332 //char _FS = get_FullScall_selection();
salco 6:86cf2afe3e52 333
salco 6:86cf2afe3e52 334 if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
salco 6:86cf2afe3e52 335 {
salco 6:86cf2afe3e52 336 result &= read_reg_short(addr_acc,OUT_X_A,&dataOut);
salco 6:86cf2afe3e52 337
salco 6:86cf2afe3e52 338 if(result)
salco 6:86cf2afe3e52 339 {
salco 6:86cf2afe3e52 340 a->x = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 341
salco 6:86cf2afe3e52 342 //setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 6:86cf2afe3e52 343 }
salco 6:86cf2afe3e52 344 else
salco 6:86cf2afe3e52 345 {
salco 6:86cf2afe3e52 346 debug("error reading \n");
salco 6:86cf2afe3e52 347 }
salco 6:86cf2afe3e52 348
salco 6:86cf2afe3e52 349 if(result)
salco 6:86cf2afe3e52 350 {
salco 6:86cf2afe3e52 351 result &= read_reg_short(addr_acc,OUT_Y_A,&dataOut);
salco 6:86cf2afe3e52 352 }
salco 6:86cf2afe3e52 353 if(result)
salco 6:86cf2afe3e52 354 {
salco 6:86cf2afe3e52 355 a->y = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 356
salco 6:86cf2afe3e52 357 //setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 6:86cf2afe3e52 358 }
salco 6:86cf2afe3e52 359 else
salco 6:86cf2afe3e52 360 {
salco 6:86cf2afe3e52 361 debug("error reading \n");
salco 6:86cf2afe3e52 362 }
salco 6:86cf2afe3e52 363
salco 6:86cf2afe3e52 364 if(result)
salco 6:86cf2afe3e52 365 {
salco 6:86cf2afe3e52 366 result &= read_reg_short(addr_acc,OUT_Z_A,&dataOut);
salco 6:86cf2afe3e52 367 }
salco 6:86cf2afe3e52 368 if(result)
salco 6:86cf2afe3e52 369 {
salco 6:86cf2afe3e52 370 a->z = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 371
salco 6:86cf2afe3e52 372 //setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 6:86cf2afe3e52 373 }
salco 6:86cf2afe3e52 374 else
salco 6:86cf2afe3e52 375 {
salco 6:86cf2afe3e52 376 debug("error reading \n");
salco 6:86cf2afe3e52 377 }
salco 6:86cf2afe3e52 378 //setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
salco 6:86cf2afe3e52 379 }
salco 6:86cf2afe3e52 380
salco 6:86cf2afe3e52 381 return result;
salco 6:86cf2afe3e52 382 }
salco 6:86cf2afe3e52 383 bool LSM303DLH::read_mag_raw(vector *m)
salco 6:86cf2afe3e52 384 {
salco 6:86cf2afe3e52 385 bool result = true;
salco 6:86cf2afe3e52 386 char data_read_mag =0;
salco 6:86cf2afe3e52 387 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 388
salco 6:86cf2afe3e52 389 read_reg(addr_mag,SR_REG_M,&data_read_mag);
salco 6:86cf2afe3e52 390
salco 6:86cf2afe3e52 391
salco 6:86cf2afe3e52 392 /**@todo not sure if the reading of magnetometer is Litle or big endian, I assume its change like the accelerometer.
salco 6:86cf2afe3e52 393 * You can try to find the answer if you care.
salco 6:86cf2afe3e52 394 */
salco 6:86cf2afe3e52 395
salco 6:86cf2afe3e52 396 if(((SR_Reg_M_t*)&data_read_mag)->DRDY)
salco 6:86cf2afe3e52 397 {
salco 6:86cf2afe3e52 398 float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
salco 6:86cf2afe3e52 399 float gainz[] = { 980., 760., 600., 400., 355., 295., 205. };
salco 6:86cf2afe3e52 400
salco 6:86cf2afe3e52 401 //setText(0,6,"GN: %d \n",_GN);
salco 6:86cf2afe3e52 402 result &= read_reg_short(addr_mag,OUT_X_M,&dataOut);
salco 6:86cf2afe3e52 403 if(result)
salco 6:86cf2afe3e52 404 {
salco 6:86cf2afe3e52 405 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 406
salco 6:86cf2afe3e52 407 setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 408 m->x = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 409 }
salco 6:86cf2afe3e52 410
salco 6:86cf2afe3e52 411 result &= read_reg_short(addr_mag,OUT_Y_M,&dataOut);
salco 6:86cf2afe3e52 412 if(result)
salco 6:86cf2afe3e52 413 {
salco 6:86cf2afe3e52 414 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 415
salco 6:86cf2afe3e52 416 setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 417 m->y = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 418 }
salco 6:86cf2afe3e52 419
salco 6:86cf2afe3e52 420 result &= read_reg_short(addr_mag,OUT_Z_M,&dataOut);
salco 6:86cf2afe3e52 421 if(result)
salco 6:86cf2afe3e52 422 {
salco 6:86cf2afe3e52 423 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 424
salco 6:86cf2afe3e52 425 setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 426 m->z = float(dataOut.value) / gainz[m_GN];
salco 6:86cf2afe3e52 427 }
salco 6:86cf2afe3e52 428 setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",m->x,m->y,m->z);
salco 6:86cf2afe3e52 429 }
salco 6:86cf2afe3e52 430
salco 6:86cf2afe3e52 431 return result;
salco 6:86cf2afe3e52 432 }