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@6:86cf2afe3e52, 2017-08-06 (annotated)

Committer:
salco
Date:
Sun Aug 06 22:23:24 2017 +0000
Revision:
6:86cf2afe3e52
Parent:
5:48722ae56546
Child:
7:275a0a69cff5
Cleanup done;

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
shimniok 0:de767f4959ef 4 *
shimniok 0:de767f4959ef 5 * Based on test program by @tosihisa and
shimniok 0:de767f4959ef 6 *
shimniok 0:de767f4959ef 7 * Pololu sample library for LSM303DLH breakout by ryantm:
shimniok 0:de767f4959ef 8 *
shimniok 0:de767f4959ef 9 * Copyright (c) 2011 Pololu Corporation. For more information, see
shimniok 0:de767f4959ef 10 *
shimniok 0:de767f4959ef 11 * http://www.pololu.com/
shimniok 0:de767f4959ef 12 * http://forum.pololu.com/
shimniok 0:de767f4959ef 13 *
shimniok 0:de767f4959ef 14 * Permission is hereby granted, free of charge, to any person
shimniok 0:de767f4959ef 15 * obtaining a copy of this software and associated documentation
shimniok 0:de767f4959ef 16 * files (the "Software"), to deal in the Software without
shimniok 0:de767f4959ef 17 * restriction, including without limitation the rights to use,
shimniok 0:de767f4959ef 18 * copy, modify, merge, publish, distribute, sublicense, and/or sell
shimniok 0:de767f4959ef 19 * copies of the Software, and to permit persons to whom the
shimniok 0:de767f4959ef 20 * Software is furnished to do so, subject to the following
shimniok 0:de767f4959ef 21 * conditions:
shimniok 0:de767f4959ef 22 *
shimniok 0:de767f4959ef 23 * The above copyright notice and this permission notice shall be
shimniok 0:de767f4959ef 24 * included in all copies or substantial portions of the Software.
shimniok 0:de767f4959ef 25 *
shimniok 0:de767f4959ef 26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
shimniok 0:de767f4959ef 27 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
shimniok 0:de767f4959ef 28 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
shimniok 0:de767f4959ef 29 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
shimniok 0:de767f4959ef 30 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
shimniok 0:de767f4959ef 31 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
shimniok 0:de767f4959ef 32 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
shimniok 0:de767f4959ef 33 * OTHER DEALINGS IN THE SOFTWARE.
shimniok 0:de767f4959ef 34 */
shimniok 0:de767f4959ef 35 #include "mbed.h"
shimniok 0:de767f4959ef 36 #include "LSM303DLH.h"
shimniok 0:de767f4959ef 37
shimniok 0:de767f4959ef 38 #ifndef M_PI
shimniok 0:de767f4959ef 39 #define M_PI 3.14159265358979323846
shimniok 0:de767f4959ef 40 #endif
shimniok 0:de767f4959ef 41
shimniok 1:48d83c63d1d9 42 #define FILTER_SHIFT 6 // used in filtering acceleromter readings
shimniok 1:48d83c63d1d9 43
salco 6:86cf2afe3e52 44
shimniok 0:de767f4959ef 45
salco 5:48722ae56546 46
salco 5:48722ae56546 47
salco 5:48722ae56546 48 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, uint8_t v)
salco 5:48722ae56546 49 {
salco 5:48722ae56546 50 return this->write_reg(addr_i2c,addr_reg,(char)v);
salco 5:48722ae56546 51 }
shimniok 0:de767f4959ef 52
shimniok 0:de767f4959ef 53 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
shimniok 0:de767f4959ef 54 {
salco 3:9b4ff901b5c9 55 bool result=false;
salco 3:9b4ff901b5c9 56 char data[2] = {addr_reg, v};
salco 3:9b4ff901b5c9 57 //__disable_irq();
salco 3:9b4ff901b5c9 58 result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
salco 3:9b4ff901b5c9 59 if(result == false) debug("Unable to Write \n");
salco 3:9b4ff901b5c9 60
salco 3:9b4ff901b5c9 61 //__enable_irq();
salco 3:9b4ff901b5c9 62 return result;
shimniok 0:de767f4959ef 63 }
shimniok 0:de767f4959ef 64
salco 5:48722ae56546 65 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, uint8_t *v)
salco 5:48722ae56546 66 {
salco 5:48722ae56546 67 return this->read_reg(addr_i2c,addr_reg,(char*)v);
salco 5:48722ae56546 68 }
salco 5:48722ae56546 69
salco 5:48722ae56546 70
shimniok 0:de767f4959ef 71 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
shimniok 0:de767f4959ef 72 {
shimniok 0:de767f4959ef 73 char data = addr_reg;
shimniok 2:aea5caec809c 74 bool result = false;
shimniok 2:aea5caec809c 75
salco 3:9b4ff901b5c9 76 //__disable_irq();
salco 3:9b4ff901b5c9 77 if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 78 {
salco 3:9b4ff901b5c9 79 if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 80 {
salco 3:9b4ff901b5c9 81 *v = data;
salco 3:9b4ff901b5c9 82 result = true;
salco 3:9b4ff901b5c9 83 }
salco 3:9b4ff901b5c9 84 else
salco 3:9b4ff901b5c9 85 {
salco 3:9b4ff901b5c9 86 debug("Unable to Read \n");
salco 3:9b4ff901b5c9 87 }
shimniok 0:de767f4959ef 88 }
salco 3:9b4ff901b5c9 89 else
salco 3:9b4ff901b5c9 90 {
salco 3:9b4ff901b5c9 91 debug("Unable to Write \n");
salco 3:9b4ff901b5c9 92 }
salco 3:9b4ff901b5c9 93 //__enable_irq();
shimniok 2:aea5caec809c 94 return result;
shimniok 0:de767f4959ef 95 }
shimniok 0:de767f4959ef 96
shimniok 0:de767f4959ef 97 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
shimniok 0:de767f4959ef 98 {
salco 3:9b4ff901b5c9 99
shimniok 0:de767f4959ef 100 char *pv = (char *)v;
shimniok 2:aea5caec809c 101 bool result;
shimniok 2:aea5caec809c 102
shimniok 2:aea5caec809c 103 result = read_reg(addr_i2c,addr_reg+0,pv+1);
shimniok 2:aea5caec809c 104 result &= read_reg(addr_i2c,addr_reg+1,pv+0);
shimniok 2:aea5caec809c 105
salco 3:9b4ff901b5c9 106
shimniok 2:aea5caec809c 107 return result;
shimniok 0:de767f4959ef 108 }
shimniok 0:de767f4959ef 109
salco 6:86cf2afe3e52 110 bool LSM303DLH::read_reg_short(DEV_ADDRS addr_i2c,REG_ADDRS addr_reg, OUT_XYZ_t *dataRead)
salco 6:86cf2afe3e52 111 {
salco 6:86cf2afe3e52 112 bool result=true;
salco 6:86cf2afe3e52 113
salco 6:86cf2afe3e52 114 switch(addr_reg)
salco 6:86cf2afe3e52 115 {
salco 6:86cf2afe3e52 116 case OUT_X_A:
salco 6:86cf2afe3e52 117 case OUT_Y_A:
salco 6:86cf2afe3e52 118 case OUT_Z_A:
salco 6:86cf2afe3e52 119
salco 6:86cf2afe3e52 120 case OUT_X_M:
salco 6:86cf2afe3e52 121 case OUT_Y_M:
salco 6:86cf2afe3e52 122 case OUT_Z_M:
salco 6:86cf2afe3e52 123 result &= read_reg(addr_i2c,addr_reg ,&(dataRead->UT_L_A));//LSB at lower
salco 6:86cf2afe3e52 124 result &= read_reg(addr_i2c,addr_reg+1,&(dataRead->UT_H_A));
salco 6:86cf2afe3e52 125 #if defined(LSM303_LITLE_ENDIAN)
salco 6:86cf2afe3e52 126 dataRead.value = (dataRead.byte[1]<<8)+(dataRead.byte[0]); //swap the reading
salco 6:86cf2afe3e52 127 #endif
salco 6:86cf2afe3e52 128 break;
salco 6:86cf2afe3e52 129 default:
salco 6:86cf2afe3e52 130 result = false;
salco 6:86cf2afe3e52 131 break;
salco 6:86cf2afe3e52 132 }
salco 6:86cf2afe3e52 133
salco 6:86cf2afe3e52 134 return result;
salco 6:86cf2afe3e52 135 }
salco 6:86cf2afe3e52 136
salco 6:86cf2afe3e52 137 LSM303DLH::~LSM303DLH()
salco 6:86cf2afe3e52 138 {
salco 6:86cf2afe3e52 139 if(m_have_createdI2C)
salco 6:86cf2afe3e52 140 {
salco 6:86cf2afe3e52 141 delete m_ptr_I2C;
salco 6:86cf2afe3e52 142 }
salco 6:86cf2afe3e52 143 }
salco 3:9b4ff901b5c9 144 LSM303DLH::LSM303DLH(PinName sda, PinName scl)
salco 3:9b4ff901b5c9 145 {
salco 3:9b4ff901b5c9 146 m_ptr_I2C = new I2C(sda, scl);
salco 6:86cf2afe3e52 147 m_have_createdI2C = true;
salco 6:86cf2afe3e52 148 this->init();
salco 3:9b4ff901b5c9 149 }
salco 3:9b4ff901b5c9 150 LSM303DLH::LSM303DLH(I2C* ptrI2C)
salco 3:9b4ff901b5c9 151 {
salco 3:9b4ff901b5c9 152 m_ptr_I2C = ptrI2C;
salco 6:86cf2afe3e52 153 m_have_createdI2C = false;
salco 3:9b4ff901b5c9 154 this->init();
salco 3:9b4ff901b5c9 155 }
salco 3:9b4ff901b5c9 156
salco 3:9b4ff901b5c9 157 void LSM303DLH::init(void)
shimniok 0:de767f4959ef 158 {
shimniok 0:de767f4959ef 159 char reg_v;
salco 3:9b4ff901b5c9 160
salco 3:9b4ff901b5c9 161 _offset_x = 0;
salco 3:9b4ff901b5c9 162 _offset_y = 0;
salco 3:9b4ff901b5c9 163 _offset_z = 0;
salco 3:9b4ff901b5c9 164 _scale_x = 0;
salco 3:9b4ff901b5c9 165 _scale_y = 0;
salco 3:9b4ff901b5c9 166 _scale_z = 0;
salco 3:9b4ff901b5c9 167 _filt_ax = 0;
salco 3:9b4ff901b5c9 168 _filt_ay = 0;
salco 3:9b4ff901b5c9 169 _filt_az = 6000;
salco 3:9b4ff901b5c9 170
salco 3:9b4ff901b5c9 171
salco 3:9b4ff901b5c9 172 m_ptr_I2C->frequency(100000);
salco 3:9b4ff901b5c9 173
salco 4:4f2ed3f8726c 174 ((Ctrl_Reg1_A_t*)&reg_v)->byte = 0;
salco 4:4f2ed3f8726c 175 ((Ctrl_Reg1_A_t*)&reg_v)->ODR |= 0b0010; /* Normal mode */
salco 4:4f2ed3f8726c 176 ((Ctrl_Reg1_A_t*)&reg_v)->Xen |= 1; /* X/Y/Z axis enable. */
salco 4:4f2ed3f8726c 177 ((Ctrl_Reg1_A_t*)&reg_v)->Yen |= 1;
salco 4:4f2ed3f8726c 178 ((Ctrl_Reg1_A_t*)&reg_v)->Zen |= 1;
shimniok 0:de767f4959ef 179 write_reg(addr_acc,CTRL_REG1_A,reg_v);
salco 4:4f2ed3f8726c 180 //not sure if we need to read the register
salco 4:4f2ed3f8726c 181 //reg_v = 0;
salco 4:4f2ed3f8726c 182 //read_reg(addr_acc,CTRL_REG1_A,&reg_v);
shimniok 0:de767f4959ef 183
salco 6:86cf2afe3e52 184 ((Ctrl_Reg4_A_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 185 ((Ctrl_Reg4_A_t*)&reg_v)->BDU |= 1; //full read befor update
salco 6:86cf2afe3e52 186 //(((Ctrl_Reg4_A_t*)&reg_v)->HR) |= 1; //hi res
salco 6:86cf2afe3e52 187 #if defined(LSM303_LITLE_ENDIAN)
salco 6:86cf2afe3e52 188 ((Ctrl_Reg4_A_t*)&reg_v)->BLE |= 1; /* 1: data MSB @ lower address */
salco 6:86cf2afe3e52 189 #endif
salco 6:86cf2afe3e52 190 ((Ctrl_Reg4_A_t*)&reg_v)->FS |= 0b01; ; /* +/- 4g */
shimniok 0:de767f4959ef 191 write_reg(addr_acc,CTRL_REG4_A,reg_v);
shimniok 0:de767f4959ef 192
shimniok 0:de767f4959ef 193 /* -- mag --- */
salco 3:9b4ff901b5c9 194 debug("in MAG \n");
salco 6:86cf2afe3e52 195 ((CRA_REG_M_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 196 ((CRA_REG_M_t*)&reg_v)->DO |= 0b100; /* Minimum data output rate = 15Hz */
shimniok 0:de767f4959ef 197 write_reg(addr_mag,CRA_REG_M,reg_v);
shimniok 0:de767f4959ef 198
shimniok 0:de767f4959ef 199 reg_v = 0;
shimniok 0:de767f4959ef 200 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
salco 6:86cf2afe3e52 201 ((CRB_REG_M_t*)&reg_v)->GN |= 0b111; /* +-8.1Gauss */
shimniok 0:de767f4959ef 202 write_reg(addr_mag,CRB_REG_M,reg_v);
shimniok 0:de767f4959ef 203
salco 6:86cf2afe3e52 204 ((MR_REG_M_t*)&reg_v)->byte = 0;
salco 6:86cf2afe3e52 205 //((MR_REG_M_t*)&reg_v)->MD |= 0; /* Continuous-conversion mode */
shimniok 0:de767f4959ef 206 write_reg(addr_mag,MR_REG_M,reg_v);
salco 3:9b4ff901b5c9 207
salco 6:86cf2afe3e52 208 //put here since we dont change it during the execution
salco 6:86cf2afe3e52 209 m_FS = get_FullScall_selection();
salco 6:86cf2afe3e52 210
salco 6:86cf2afe3e52 211 read_reg(addr_mag,CRB_REG_M ,&reg_v);
salco 6:86cf2afe3e52 212 m_GN = (((CRB_REG_M_t*)&reg_v)->GN)-1;
shimniok 0:de767f4959ef 213 }
shimniok 0:de767f4959ef 214
shimniok 0:de767f4959ef 215 void LSM303DLH::setOffset(float x, float y, float z)
shimniok 0:de767f4959ef 216 {
shimniok 0:de767f4959ef 217 _offset_x = x;
shimniok 0:de767f4959ef 218 _offset_y = y;
shimniok 0:de767f4959ef 219 _offset_z = z;
shimniok 0:de767f4959ef 220 }
shimniok 0:de767f4959ef 221
shimniok 0:de767f4959ef 222 void LSM303DLH::setScale(float x, float y, float z)
shimniok 0:de767f4959ef 223 {
shimniok 0:de767f4959ef 224 _scale_x = x;
shimniok 0:de767f4959ef 225 _scale_y = y;
shimniok 0:de767f4959ef 226 _scale_z = z;
shimniok 0:de767f4959ef 227 }
salco 5:48722ae56546 228 //#define _FS 4
salco 3:9b4ff901b5c9 229 bool LSM303DLH::read(vector &a, vector &m)
shimniok 0:de767f4959ef 230 {
salco 3:9b4ff901b5c9 231
salco 3:9b4ff901b5c9 232 bool result = true;
salco 5:48722ae56546 233 //short a_x, a_y, a_z;
salco 6:86cf2afe3e52 234 //short m_x, m_y, m_z;
salco 3:9b4ff901b5c9 235 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 236 Timer t;
salco 3:9b4ff901b5c9 237 int usec1, usec2;
salco 3:9b4ff901b5c9 238
salco 3:9b4ff901b5c9 239 t.reset();
salco 3:9b4ff901b5c9 240 t.start();
shimniok 2:aea5caec809c 241
salco 3:9b4ff901b5c9 242 usec1 = t.read_us();
salco 3:9b4ff901b5c9 243 #endif
salco 3:9b4ff901b5c9 244
salco 6:86cf2afe3e52 245 //OUT_XYZ_t my_test,dataOut;
salco 6:86cf2afe3e52 246 static vector a_test, m_test;
shimniok 0:de767f4959ef 247
salco 6:86cf2afe3e52 248 result &= read_acc_raw(&a_test);
salco 5:48722ae56546 249
salco 6:86cf2afe3e52 250 result &= read_mag_raw(&m_test);
salco 6:86cf2afe3e52 251
salco 6:86cf2afe3e52 252
salco 3:9b4ff901b5c9 253 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 254 usec2 = t.read_us();
salco 3:9b4ff901b5c9 255
salco 3:9b4ff901b5c9 256 debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
salco 3:9b4ff901b5c9 257 #endif
salco 3:9b4ff901b5c9 258 if(result == true)
salco 3:9b4ff901b5c9 259 {
salco 3:9b4ff901b5c9 260 // Perform simple lowpass filtering
salco 3:9b4ff901b5c9 261 // Intended to stabilize heading despite
salco 3:9b4ff901b5c9 262 // device vibration such as on a UGV
salco 3:9b4ff901b5c9 263
salco 6:86cf2afe3e52 264
salco 3:9b4ff901b5c9 265 //float( a[i] ) * pow(2.,(fs+1)) / 32768.
salco 3:9b4ff901b5c9 266
salco 3:9b4ff901b5c9 267 //x/8 = reading /0xFFFF(655355)
salco 3:9b4ff901b5c9 268
salco 3:9b4ff901b5c9 269
salco 3:9b4ff901b5c9 270
salco 3:9b4ff901b5c9 271 // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
salco 5:48722ae56546 272 /* _filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
salco 3:9b4ff901b5c9 273 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 274 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 275
salco 3:9b4ff901b5c9 276
salco 3:9b4ff901b5c9 277
salco 3:9b4ff901b5c9 278 a.x = (float) (_filt_ax >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 279 a.y = (float) (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 280 a.z = (float) (_filt_az >> FILTER_SHIFT);*/
salco 6:86cf2afe3e52 281
salco 6:86cf2afe3e52 282 a = a_test;
salco 6:86cf2afe3e52 283
salco 6:86cf2afe3e52 284
salco 6:86cf2afe3e52 285 // offset and scale
salco 6:86cf2afe3e52 286 m.x = (/*m_*/m_test.x + _offset_x) * _scale_x;
salco 6:86cf2afe3e52 287 m.y = (/*m_*/m_test.y + _offset_y) * _scale_y;
salco 6:86cf2afe3e52 288 m.z = (/*m_*/m_test.z + _offset_z) * _scale_z;
salco 6:86cf2afe3e52 289
salco 3:9b4ff901b5c9 290 }
salco 3:9b4ff901b5c9 291
salco 3:9b4ff901b5c9 292 return result;
shimniok 0:de767f4959ef 293 }
shimniok 0:de767f4959ef 294
shimniok 0:de767f4959ef 295
shimniok 0:de767f4959ef 296 // Returns the number of degrees from the -Y axis that it
shimniok 0:de767f4959ef 297 // is pointing.
shimniok 0:de767f4959ef 298 float LSM303DLH::heading()
shimniok 0:de767f4959ef 299 {
shimniok 0:de767f4959ef 300 return heading((vector){0,-1,0});
shimniok 0:de767f4959ef 301 }
shimniok 0:de767f4959ef 302
shimniok 0:de767f4959ef 303 float LSM303DLH::heading(vector from)
shimniok 0:de767f4959ef 304 {
shimniok 0:de767f4959ef 305 vector a, m;
shimniok 0:de767f4959ef 306
salco 3:9b4ff901b5c9 307 read(a, m);
shimniok 0:de767f4959ef 308
shimniok 0:de767f4959ef 309 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 310 // compute heading
shimniok 0:de767f4959ef 311 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 312
shimniok 0:de767f4959ef 313 vector temp_a = a;
shimniok 0:de767f4959ef 314 // normalize
shimniok 0:de767f4959ef 315 vector_normalize(&temp_a);
shimniok 0:de767f4959ef 316 //vector_normalize(&m);
shimniok 0:de767f4959ef 317
shimniok 0:de767f4959ef 318 // compute E and N
shimniok 0:de767f4959ef 319 vector E;
shimniok 0:de767f4959ef 320 vector N;
shimniok 0:de767f4959ef 321 vector_cross(&m,&temp_a,&E);
shimniok 0:de767f4959ef 322 vector_normalize(&E);
shimniok 0:de767f4959ef 323 vector_cross(&temp_a,&E,&N);
shimniok 0:de767f4959ef 324
shimniok 0:de767f4959ef 325 // compute heading
shimniok 0:de767f4959ef 326 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
shimniok 0:de767f4959ef 327 if (heading < 0) heading += 360;
shimniok 0:de767f4959ef 328
shimniok 0:de767f4959ef 329 return heading;
shimniok 0:de767f4959ef 330 }
shimniok 2:aea5caec809c 331
shimniok 2:aea5caec809c 332 void LSM303DLH::frequency(int hz)
shimniok 2:aea5caec809c 333 {
salco 3:9b4ff901b5c9 334 m_ptr_I2C->frequency(hz);
salco 3:9b4ff901b5c9 335 }
salco 5:48722ae56546 336
salco 5:48722ae56546 337 int8_t LSM303DLH::get_FullScall_selection(void)
salco 5:48722ae56546 338 {
salco 5:48722ae56546 339 char data_read_acc =0;
salco 5:48722ae56546 340 read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
salco 5:48722ae56546 341
salco 5:48722ae56546 342 return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
salco 5:48722ae56546 343 }
salco 6:86cf2afe3e52 344
salco 6:86cf2afe3e52 345 float LSM303DLH::get_acc_value_in_g(OUT_XYZ_t* dataOut)
salco 6:86cf2afe3e52 346 {
salco 6:86cf2afe3e52 347 return (float) (dataOut->value / (float)(32768 /*half of the ADC resolution*/ / m_FS/*+- 4g*/));
salco 6:86cf2afe3e52 348 }
salco 6:86cf2afe3e52 349
salco 6:86cf2afe3e52 350 bool LSM303DLH::read_acc_raw(vector *a)
salco 6:86cf2afe3e52 351 {
salco 6:86cf2afe3e52 352 bool result = true;
salco 6:86cf2afe3e52 353 char data_read_acc =0;
salco 6:86cf2afe3e52 354 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 355
salco 6:86cf2afe3e52 356 read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
salco 6:86cf2afe3e52 357 //char _FS = get_FullScall_selection();
salco 6:86cf2afe3e52 358
salco 6:86cf2afe3e52 359 if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
salco 6:86cf2afe3e52 360 {
salco 6:86cf2afe3e52 361 result &= read_reg_short(addr_acc,OUT_X_A,&dataOut);
salco 6:86cf2afe3e52 362
salco 6:86cf2afe3e52 363 if(result)
salco 6:86cf2afe3e52 364 {
salco 6:86cf2afe3e52 365 a->x = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 366
salco 6:86cf2afe3e52 367 //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 368 }
salco 6:86cf2afe3e52 369 else
salco 6:86cf2afe3e52 370 {
salco 6:86cf2afe3e52 371 debug("error reading \n");
salco 6:86cf2afe3e52 372 }
salco 6:86cf2afe3e52 373
salco 6:86cf2afe3e52 374 if(result)
salco 6:86cf2afe3e52 375 {
salco 6:86cf2afe3e52 376 result &= read_reg_short(addr_acc,OUT_Y_A,&dataOut);
salco 6:86cf2afe3e52 377 }
salco 6:86cf2afe3e52 378 if(result)
salco 6:86cf2afe3e52 379 {
salco 6:86cf2afe3e52 380 a->y = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 381
salco 6:86cf2afe3e52 382 //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 383 }
salco 6:86cf2afe3e52 384 else
salco 6:86cf2afe3e52 385 {
salco 6:86cf2afe3e52 386 debug("error reading \n");
salco 6:86cf2afe3e52 387 }
salco 6:86cf2afe3e52 388
salco 6:86cf2afe3e52 389 if(result)
salco 6:86cf2afe3e52 390 {
salco 6:86cf2afe3e52 391 result &= read_reg_short(addr_acc,OUT_Z_A,&dataOut);
salco 6:86cf2afe3e52 392 }
salco 6:86cf2afe3e52 393 if(result)
salco 6:86cf2afe3e52 394 {
salco 6:86cf2afe3e52 395 a->z = get_acc_value_in_g(&dataOut);
salco 6:86cf2afe3e52 396
salco 6:86cf2afe3e52 397 //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 398 }
salco 6:86cf2afe3e52 399 else
salco 6:86cf2afe3e52 400 {
salco 6:86cf2afe3e52 401 debug("error reading \n");
salco 6:86cf2afe3e52 402 }
salco 6:86cf2afe3e52 403 //setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
salco 6:86cf2afe3e52 404 }
salco 6:86cf2afe3e52 405
salco 6:86cf2afe3e52 406 return result;
salco 6:86cf2afe3e52 407 }
salco 6:86cf2afe3e52 408 bool LSM303DLH::read_mag_raw(vector *m)
salco 6:86cf2afe3e52 409 {
salco 6:86cf2afe3e52 410 bool result = true;
salco 6:86cf2afe3e52 411 char data_read_mag =0;
salco 6:86cf2afe3e52 412 OUT_XYZ_t dataOut;
salco 6:86cf2afe3e52 413
salco 6:86cf2afe3e52 414 read_reg(addr_mag,SR_REG_M,&data_read_mag);
salco 6:86cf2afe3e52 415
salco 6:86cf2afe3e52 416
salco 6:86cf2afe3e52 417 /**@todo not sure if the reading of magnetometer is Litle or big endian, I assume its change like the accelerometer.
salco 6:86cf2afe3e52 418 * You can try to find the answer if you care.
salco 6:86cf2afe3e52 419 */
salco 6:86cf2afe3e52 420
salco 6:86cf2afe3e52 421 if(((SR_Reg_M_t*)&data_read_mag)->DRDY)
salco 6:86cf2afe3e52 422 {
salco 6:86cf2afe3e52 423 float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
salco 6:86cf2afe3e52 424 float gainz[] = { 980., 760., 600., 400., 355., 295., 205. };
salco 6:86cf2afe3e52 425
salco 6:86cf2afe3e52 426 //setText(0,6,"GN: %d \n",_GN);
salco 6:86cf2afe3e52 427 result &= read_reg_short(addr_mag,OUT_X_M,&dataOut);
salco 6:86cf2afe3e52 428 if(result)
salco 6:86cf2afe3e52 429 {
salco 6:86cf2afe3e52 430 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 431
salco 6:86cf2afe3e52 432 setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 433 m->x = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 434 }
salco 6:86cf2afe3e52 435
salco 6:86cf2afe3e52 436 result &= read_reg_short(addr_mag,OUT_Y_M,&dataOut);
salco 6:86cf2afe3e52 437 if(result)
salco 6:86cf2afe3e52 438 {
salco 6:86cf2afe3e52 439 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 440
salco 6:86cf2afe3e52 441 setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 442 m->y = float(dataOut.value) / gainxy[m_GN];
salco 6:86cf2afe3e52 443 }
salco 6:86cf2afe3e52 444
salco 6:86cf2afe3e52 445 result &= read_reg_short(addr_mag,OUT_Z_M,&dataOut);
salco 6:86cf2afe3e52 446 if(result)
salco 6:86cf2afe3e52 447 {
salco 6:86cf2afe3e52 448 //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
salco 6:86cf2afe3e52 449
salco 6:86cf2afe3e52 450 setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
salco 6:86cf2afe3e52 451 m->z = float(dataOut.value) / gainz[m_GN];
salco 6:86cf2afe3e52 452 }
salco 6:86cf2afe3e52 453 setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",m->x,m->y,m->z);
salco 6:86cf2afe3e52 454 }
salco 6:86cf2afe3e52 455
salco 6:86cf2afe3e52 456 return result;
salco 6:86cf2afe3e52 457 }