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@5:48722ae56546, 2017-08-06 (annotated)

Committer:
salco
Date:
Sun Aug 06 19:11:23 2017 +0000
Revision:
5:48722ae56546
Parent:
4:4f2ed3f8726c
Child:
6:86cf2afe3e52
a new morning;

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 3:9b4ff901b5c9 44 const int addr_acc = 0x32;//0x30;
shimniok 0:de767f4959ef 45 const int addr_mag = 0x3c;
shimniok 0:de767f4959ef 46
salco 5:48722ae56546 47
salco 5:48722ae56546 48
salco 5:48722ae56546 49 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, uint8_t v)
salco 5:48722ae56546 50 {
salco 5:48722ae56546 51 return this->write_reg(addr_i2c,addr_reg,(char)v);
salco 5:48722ae56546 52 }
shimniok 0:de767f4959ef 53
shimniok 0:de767f4959ef 54 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
shimniok 0:de767f4959ef 55 {
salco 3:9b4ff901b5c9 56 bool result=false;
salco 3:9b4ff901b5c9 57 char data[2] = {addr_reg, v};
salco 3:9b4ff901b5c9 58 //__disable_irq();
salco 3:9b4ff901b5c9 59 result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
salco 3:9b4ff901b5c9 60 if(result == false) debug("Unable to Write \n");
salco 3:9b4ff901b5c9 61
salco 3:9b4ff901b5c9 62 //__enable_irq();
salco 3:9b4ff901b5c9 63 return result;
shimniok 0:de767f4959ef 64 }
shimniok 0:de767f4959ef 65
salco 5:48722ae56546 66 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, uint8_t *v)
salco 5:48722ae56546 67 {
salco 5:48722ae56546 68 return this->read_reg(addr_i2c,addr_reg,(char*)v);
salco 5:48722ae56546 69 }
salco 5:48722ae56546 70
salco 5:48722ae56546 71
shimniok 0:de767f4959ef 72 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
shimniok 0:de767f4959ef 73 {
shimniok 0:de767f4959ef 74 char data = addr_reg;
shimniok 2:aea5caec809c 75 bool result = false;
shimniok 2:aea5caec809c 76
salco 3:9b4ff901b5c9 77 //__disable_irq();
salco 3:9b4ff901b5c9 78 if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 79 {
salco 3:9b4ff901b5c9 80 if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)
salco 3:9b4ff901b5c9 81 {
salco 3:9b4ff901b5c9 82 *v = data;
salco 3:9b4ff901b5c9 83 result = true;
salco 3:9b4ff901b5c9 84 }
salco 3:9b4ff901b5c9 85 else
salco 3:9b4ff901b5c9 86 {
salco 3:9b4ff901b5c9 87 debug("Unable to Read \n");
salco 3:9b4ff901b5c9 88 }
shimniok 0:de767f4959ef 89 }
salco 3:9b4ff901b5c9 90 else
salco 3:9b4ff901b5c9 91 {
salco 3:9b4ff901b5c9 92 debug("Unable to Write \n");
salco 3:9b4ff901b5c9 93 }
salco 3:9b4ff901b5c9 94 //__enable_irq();
shimniok 2:aea5caec809c 95 return result;
shimniok 0:de767f4959ef 96 }
shimniok 0:de767f4959ef 97
shimniok 0:de767f4959ef 98 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
shimniok 0:de767f4959ef 99 {
salco 3:9b4ff901b5c9 100
shimniok 0:de767f4959ef 101 char *pv = (char *)v;
shimniok 2:aea5caec809c 102 bool result;
shimniok 2:aea5caec809c 103
shimniok 2:aea5caec809c 104 result = read_reg(addr_i2c,addr_reg+0,pv+1);
shimniok 2:aea5caec809c 105 result &= read_reg(addr_i2c,addr_reg+1,pv+0);
shimniok 2:aea5caec809c 106
salco 3:9b4ff901b5c9 107
shimniok 2:aea5caec809c 108 return result;
shimniok 0:de767f4959ef 109 }
shimniok 0:de767f4959ef 110
salco 3:9b4ff901b5c9 111 LSM303DLH::LSM303DLH(PinName sda, PinName scl)
salco 3:9b4ff901b5c9 112 {
salco 3:9b4ff901b5c9 113 m_ptr_I2C = new I2C(sda, scl);
salco 3:9b4ff901b5c9 114 init();
salco 3:9b4ff901b5c9 115 }
salco 3:9b4ff901b5c9 116 LSM303DLH::LSM303DLH(I2C* ptrI2C)
salco 3:9b4ff901b5c9 117 {
salco 3:9b4ff901b5c9 118 m_ptr_I2C = ptrI2C;
salco 3:9b4ff901b5c9 119 //debug( "ptr pass %d, ptr here %d \n",(int)ptrI2C,(int)m_ptr_I2C);
salco 3:9b4ff901b5c9 120 this->init();
salco 3:9b4ff901b5c9 121 }
salco 3:9b4ff901b5c9 122
salco 3:9b4ff901b5c9 123 void LSM303DLH::init(void)
shimniok 0:de767f4959ef 124 {
shimniok 0:de767f4959ef 125 char reg_v;
salco 3:9b4ff901b5c9 126
salco 3:9b4ff901b5c9 127 debug("In INIT \n");
salco 3:9b4ff901b5c9 128 _offset_x = 0;
salco 3:9b4ff901b5c9 129 _offset_y = 0;
salco 3:9b4ff901b5c9 130 _offset_z = 0;
salco 3:9b4ff901b5c9 131 _scale_x = 0;
salco 3:9b4ff901b5c9 132 _scale_y = 0;
salco 3:9b4ff901b5c9 133 _scale_z = 0;
salco 3:9b4ff901b5c9 134 _filt_ax = 0;
salco 3:9b4ff901b5c9 135 _filt_ay = 0;
salco 3:9b4ff901b5c9 136 _filt_az = 6000;
salco 3:9b4ff901b5c9 137
salco 3:9b4ff901b5c9 138
salco 3:9b4ff901b5c9 139 m_ptr_I2C->frequency(100000);
salco 3:9b4ff901b5c9 140
salco 4:4f2ed3f8726c 141 ((Ctrl_Reg1_A_t*)&reg_v)->byte = 0;
salco 4:4f2ed3f8726c 142 ((Ctrl_Reg1_A_t*)&reg_v)->ODR |= 0b0010; /* Normal mode */
salco 4:4f2ed3f8726c 143 ((Ctrl_Reg1_A_t*)&reg_v)->Xen |= 1; /* X/Y/Z axis enable. */
salco 4:4f2ed3f8726c 144 ((Ctrl_Reg1_A_t*)&reg_v)->Yen |= 1;
salco 4:4f2ed3f8726c 145 ((Ctrl_Reg1_A_t*)&reg_v)->Zen |= 1;
shimniok 0:de767f4959ef 146 write_reg(addr_acc,CTRL_REG1_A,reg_v);
salco 4:4f2ed3f8726c 147 //not sure if we need to read the register
salco 4:4f2ed3f8726c 148 //reg_v = 0;
salco 4:4f2ed3f8726c 149 //read_reg(addr_acc,CTRL_REG1_A,&reg_v);
shimniok 0:de767f4959ef 150
shimniok 0:de767f4959ef 151 reg_v = 0;
salco 3:9b4ff901b5c9 152 reg_v |= 0x01 << 7; //full read
salco 4:4f2ed3f8726c 153 //reg_v |= 0x01 << 3; //hi res
salco 3:9b4ff901b5c9 154 //reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */
shimniok 0:de767f4959ef 155 reg_v |= 0x01 << 4; /* +/- 4g */
shimniok 0:de767f4959ef 156 write_reg(addr_acc,CTRL_REG4_A,reg_v);
shimniok 0:de767f4959ef 157
shimniok 0:de767f4959ef 158 /* -- mag --- */
salco 3:9b4ff901b5c9 159 debug("in MAG \n");
shimniok 0:de767f4959ef 160 reg_v = 0;
shimniok 0:de767f4959ef 161 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
shimniok 0:de767f4959ef 162 write_reg(addr_mag,CRA_REG_M,reg_v);
shimniok 0:de767f4959ef 163
shimniok 0:de767f4959ef 164 reg_v = 0;
shimniok 0:de767f4959ef 165 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
salco 5:48722ae56546 166 (((CRB_REG_M_t*)&reg_v)->GN) |= 0b111; /* +-8.1Gauss */
shimniok 0:de767f4959ef 167 write_reg(addr_mag,CRB_REG_M,reg_v);
shimniok 0:de767f4959ef 168
shimniok 0:de767f4959ef 169 reg_v = 0; /* Continuous-conversion mode */
shimniok 0:de767f4959ef 170 write_reg(addr_mag,MR_REG_M,reg_v);
salco 3:9b4ff901b5c9 171
salco 3:9b4ff901b5c9 172 debug("Out INIT \n");
salco 3:9b4ff901b5c9 173 clearscreen();
shimniok 0:de767f4959ef 174 }
shimniok 0:de767f4959ef 175
shimniok 0:de767f4959ef 176 void LSM303DLH::setOffset(float x, float y, float z)
shimniok 0:de767f4959ef 177 {
shimniok 0:de767f4959ef 178 _offset_x = x;
shimniok 0:de767f4959ef 179 _offset_y = y;
shimniok 0:de767f4959ef 180 _offset_z = z;
shimniok 0:de767f4959ef 181 }
shimniok 0:de767f4959ef 182
shimniok 0:de767f4959ef 183 void LSM303DLH::setScale(float x, float y, float z)
shimniok 0:de767f4959ef 184 {
shimniok 0:de767f4959ef 185 _scale_x = x;
shimniok 0:de767f4959ef 186 _scale_y = y;
shimniok 0:de767f4959ef 187 _scale_z = z;
shimniok 0:de767f4959ef 188 }
salco 5:48722ae56546 189 //#define _FS 4
salco 3:9b4ff901b5c9 190 bool LSM303DLH::read(vector &a, vector &m)
shimniok 0:de767f4959ef 191 {
salco 3:9b4ff901b5c9 192
salco 3:9b4ff901b5c9 193 bool result = true;
salco 5:48722ae56546 194 //short a_x, a_y, a_z;
shimniok 0:de767f4959ef 195 short m_x, m_y, m_z;
salco 3:9b4ff901b5c9 196 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 197 Timer t;
salco 3:9b4ff901b5c9 198 int usec1, usec2;
salco 3:9b4ff901b5c9 199
salco 3:9b4ff901b5c9 200 t.reset();
salco 3:9b4ff901b5c9 201 t.start();
shimniok 2:aea5caec809c 202
salco 3:9b4ff901b5c9 203 usec1 = t.read_us();
salco 3:9b4ff901b5c9 204 #endif
salco 3:9b4ff901b5c9 205
salco 5:48722ae56546 206 /*union{
salco 3:9b4ff901b5c9 207 int16_t number;
salco 3:9b4ff901b5c9 208 char byte[2];
salco 3:9b4ff901b5c9 209 struct{
salco 3:9b4ff901b5c9 210 char MSB;
salco 3:9b4ff901b5c9 211 char LSB;
salco 3:9b4ff901b5c9 212 };
salco 5:48722ae56546 213 }*/
salco 5:48722ae56546 214 OUT_XYZ_t my_test;
salco 5:48722ae56546 215 vector a_test, m_test;
salco 3:9b4ff901b5c9 216
salco 3:9b4ff901b5c9 217 char data_read_acc =0;
salco 3:9b4ff901b5c9 218 read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
salco 5:48722ae56546 219 char _FS = get_FullScall_selection();
salco 5:48722ae56546 220
salco 5:48722ae56546 221 if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
salco 3:9b4ff901b5c9 222 {
salco 5:48722ae56546 223 result &= read_reg(addr_acc,OUT_X_A ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 224 result &= read_reg(addr_acc,OUT_X_A+1,&(my_test.UT_H_A/*LSB*/));
salco 5:48722ae56546 225
salco 3:9b4ff901b5c9 226 if(result)
salco 3:9b4ff901b5c9 227 {
salco 3:9b4ff901b5c9 228 //a_test.x = my_test.number;
salco 5:48722ae56546 229 a_test.x = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
salco 5:48722ae56546 230
salco 5:48722ae56546 231 //setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 3:9b4ff901b5c9 232 }
salco 3:9b4ff901b5c9 233 else
salco 3:9b4ff901b5c9 234 {
salco 3:9b4ff901b5c9 235 debug("error reading \n");
salco 3:9b4ff901b5c9 236 }
salco 3:9b4ff901b5c9 237
salco 3:9b4ff901b5c9 238 if(result)
salco 3:9b4ff901b5c9 239 {
salco 5:48722ae56546 240 result &= read_reg(addr_acc,OUT_Y_A ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 241 result &= read_reg(addr_acc,OUT_Y_A+1,&(my_test.UT_H_A/*LSB*/));
salco 3:9b4ff901b5c9 242 }
salco 3:9b4ff901b5c9 243 if(result)
salco 3:9b4ff901b5c9 244 {
salco 3:9b4ff901b5c9 245 //a_test.y = my_test.number;
salco 5:48722ae56546 246 a_test.y = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
salco 5:48722ae56546 247
salco 5:48722ae56546 248 //setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 3:9b4ff901b5c9 249 }
salco 3:9b4ff901b5c9 250 else
salco 3:9b4ff901b5c9 251 {
salco 3:9b4ff901b5c9 252 debug("error reading \n");
salco 3:9b4ff901b5c9 253 }
salco 3:9b4ff901b5c9 254
salco 3:9b4ff901b5c9 255 if(result)
salco 3:9b4ff901b5c9 256 {
salco 5:48722ae56546 257 result &= read_reg(addr_acc,OUT_Z_A ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 258 result &= read_reg(addr_acc,OUT_Z_A+1,&(my_test.UT_H_A/*LSB*/));
salco 3:9b4ff901b5c9 259 }
salco 3:9b4ff901b5c9 260 if(result)
salco 3:9b4ff901b5c9 261 {
salco 3:9b4ff901b5c9 262 //a_test.z = my_test.number;
salco 5:48722ae56546 263 a_test.z = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
salco 5:48722ae56546 264
salco 5:48722ae56546 265 //setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 3:9b4ff901b5c9 266 }
salco 3:9b4ff901b5c9 267 else
salco 3:9b4ff901b5c9 268 {
salco 3:9b4ff901b5c9 269 debug("error reading \n");
salco 3:9b4ff901b5c9 270 }
salco 5:48722ae56546 271 //setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
salco 3:9b4ff901b5c9 272 }
salco 3:9b4ff901b5c9 273 /*result &= read_reg_short(addr_acc, OUT_X_A, &a_x);
salco 3:9b4ff901b5c9 274 result &= read_reg_short(addr_acc, OUT_Y_A, &a_y);
salco 3:9b4ff901b5c9 275 result &= read_reg_short(addr_acc, OUT_Z_A, &a_z);*/
salco 3:9b4ff901b5c9 276 //This test pass so its ok
shimniok 0:de767f4959ef 277
salco 5:48722ae56546 278 char data_read_mag =0;
salco 5:48722ae56546 279 read_reg(addr_mag,SR_REG_M,&data_read_mag);
salco 5:48722ae56546 280
salco 5:48722ae56546 281 if(((SR_Reg_M_t*)&data_read_acc)->DRDY)
salco 5:48722ae56546 282 {
salco 5:48722ae56546 283 float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
salco 5:48722ae56546 284 float gainz[] = { 980., 760., 600., 400., 355., 295., 205. };
salco 5:48722ae56546 285 char _GN;
salco 5:48722ae56546 286 read_reg(addr_mag,CRB_REG_M ,&_GN);
salco 5:48722ae56546 287 _GN = (((CRB_REG_M_t*)&_GN)->GN)-1;
salco 5:48722ae56546 288 setText(0,6,"GN: %d \n",_GN);
salco 5:48722ae56546 289 //result &= read_reg_short(addr_mag, OUT_X_M, &m_x);
salco 5:48722ae56546 290 result &= read_reg(addr_mag,OUT_X_M ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 291 result &= read_reg(addr_mag,OUT_X_M+1,&(my_test.UT_H_A/*LSB*/));
salco 5:48722ae56546 292 //a_test.x = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
salco 5:48722ae56546 293 setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 5:48722ae56546 294 m_test.x = float(my_test.value) / gainxy[_GN];
salco 5:48722ae56546 295
salco 5:48722ae56546 296 //result &= read_reg_short(addr_mag, OUT_Y_M, &m_y);
salco 5:48722ae56546 297 result &= read_reg(addr_mag,OUT_Y_M ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 298 result &= read_reg(addr_mag,OUT_Y_M+1,&(my_test.UT_H_A/*LSB*/));
salco 5:48722ae56546 299 setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 5:48722ae56546 300 m_test.y = float(my_test.value) / gainxy[_GN];
salco 5:48722ae56546 301
salco 5:48722ae56546 302 //result &= read_reg_short(addr_mag, OUT_Z_M, &m_z);
salco 5:48722ae56546 303 result &= read_reg(addr_mag,OUT_Z_M ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
salco 5:48722ae56546 304 result &= read_reg(addr_mag,OUT_Z_M+1,&(my_test.UT_H_A/*LSB*/));
salco 5:48722ae56546 305 setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
salco 5:48722ae56546 306 m_test.z = float(my_test.value) / gainz[_GN];
salco 5:48722ae56546 307
salco 5:48722ae56546 308 setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",m_test.x,m_test.y,m_test.z);
salco 5:48722ae56546 309 }
salco 3:9b4ff901b5c9 310 #if defined(CHECK_TIME_SEQUENCE)
salco 3:9b4ff901b5c9 311 usec2 = t.read_us();
salco 3:9b4ff901b5c9 312
salco 3:9b4ff901b5c9 313 debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
salco 3:9b4ff901b5c9 314 #endif
salco 3:9b4ff901b5c9 315 if(result == true)
salco 3:9b4ff901b5c9 316 {
salco 3:9b4ff901b5c9 317 // Perform simple lowpass filtering
salco 3:9b4ff901b5c9 318 // Intended to stabilize heading despite
salco 3:9b4ff901b5c9 319 // device vibration such as on a UGV
salco 3:9b4ff901b5c9 320
salco 3:9b4ff901b5c9 321 #if 0
salco 3:9b4ff901b5c9 322 if(data_read_acc & (1<<3))
salco 3:9b4ff901b5c9 323 {
salco 3:9b4ff901b5c9 324 a_test.x= a_test.x / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
salco 3:9b4ff901b5c9 325 a_test.y= a_test.y / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
salco 3:9b4ff901b5c9 326 a_test.z= a_test.z / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
salco 3:9b4ff901b5c9 327
salco 5:48722ae56546 328
salco 3:9b4ff901b5c9 329 }
salco 3:9b4ff901b5c9 330 #endif
salco 3:9b4ff901b5c9 331 //float( a[i] ) * pow(2.,(fs+1)) / 32768.
salco 3:9b4ff901b5c9 332
salco 3:9b4ff901b5c9 333 //x/8 = reading /0xFFFF(655355)
salco 3:9b4ff901b5c9 334
salco 3:9b4ff901b5c9 335
salco 3:9b4ff901b5c9 336
salco 3:9b4ff901b5c9 337 // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
salco 5:48722ae56546 338 /* _filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
salco 3:9b4ff901b5c9 339 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 340 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 341
salco 3:9b4ff901b5c9 342
salco 3:9b4ff901b5c9 343
salco 3:9b4ff901b5c9 344 a.x = (float) (_filt_ax >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 345 a.y = (float) (_filt_ay >> FILTER_SHIFT);
salco 3:9b4ff901b5c9 346 a.z = (float) (_filt_az >> FILTER_SHIFT);*/
salco 5:48722ae56546 347 if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)
salco 5:48722ae56546 348 {
salco 5:48722ae56546 349 a.x = a_test.x;
salco 5:48722ae56546 350 a.y = a_test.y;
salco 5:48722ae56546 351 a.z = a_test.z;
salco 5:48722ae56546 352 }
salco 3:9b4ff901b5c9 353 // offset and scale
salco 5:48722ae56546 354 if(((SR_Reg_M_t*)&data_read_acc)->DRDY)
salco 5:48722ae56546 355 {
salco 5:48722ae56546 356 m.x = (/*m_*/m_test.x + _offset_x) * _scale_x;
salco 5:48722ae56546 357 m.y = (/*m_*/m_test.y + _offset_y) * _scale_y;
salco 5:48722ae56546 358 m.z = (/*m_*/m_test.z + _offset_z) * _scale_z;
salco 5:48722ae56546 359 }
salco 3:9b4ff901b5c9 360 }
salco 3:9b4ff901b5c9 361
salco 3:9b4ff901b5c9 362 return result;
shimniok 0:de767f4959ef 363 }
shimniok 0:de767f4959ef 364
shimniok 0:de767f4959ef 365
shimniok 0:de767f4959ef 366 // Returns the number of degrees from the -Y axis that it
shimniok 0:de767f4959ef 367 // is pointing.
shimniok 0:de767f4959ef 368 float LSM303DLH::heading()
shimniok 0:de767f4959ef 369 {
shimniok 0:de767f4959ef 370 return heading((vector){0,-1,0});
shimniok 0:de767f4959ef 371 }
shimniok 0:de767f4959ef 372
shimniok 0:de767f4959ef 373 float LSM303DLH::heading(vector from)
shimniok 0:de767f4959ef 374 {
shimniok 0:de767f4959ef 375 vector a, m;
shimniok 0:de767f4959ef 376
salco 3:9b4ff901b5c9 377 read(a, m);
shimniok 0:de767f4959ef 378
shimniok 0:de767f4959ef 379 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 380 // compute heading
shimniok 0:de767f4959ef 381 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 382
shimniok 0:de767f4959ef 383 vector temp_a = a;
shimniok 0:de767f4959ef 384 // normalize
shimniok 0:de767f4959ef 385 vector_normalize(&temp_a);
shimniok 0:de767f4959ef 386 //vector_normalize(&m);
shimniok 0:de767f4959ef 387
shimniok 0:de767f4959ef 388 // compute E and N
shimniok 0:de767f4959ef 389 vector E;
shimniok 0:de767f4959ef 390 vector N;
shimniok 0:de767f4959ef 391 vector_cross(&m,&temp_a,&E);
shimniok 0:de767f4959ef 392 vector_normalize(&E);
shimniok 0:de767f4959ef 393 vector_cross(&temp_a,&E,&N);
shimniok 0:de767f4959ef 394
shimniok 0:de767f4959ef 395 // compute heading
shimniok 0:de767f4959ef 396 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
shimniok 0:de767f4959ef 397 if (heading < 0) heading += 360;
shimniok 0:de767f4959ef 398
shimniok 0:de767f4959ef 399 return heading;
shimniok 0:de767f4959ef 400 }
shimniok 2:aea5caec809c 401
shimniok 2:aea5caec809c 402 void LSM303DLH::frequency(int hz)
shimniok 2:aea5caec809c 403 {
salco 3:9b4ff901b5c9 404 m_ptr_I2C->frequency(hz);
salco 3:9b4ff901b5c9 405 }
salco 5:48722ae56546 406
salco 5:48722ae56546 407 int8_t LSM303DLH::get_FullScall_selection(void)
salco 5:48722ae56546 408 {
salco 5:48722ae56546 409 char data_read_acc =0;
salco 5:48722ae56546 410 read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
salco 5:48722ae56546 411
salco 5:48722ae56546 412 return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
salco 5:48722ae56546 413 }