My attempt to made a more useful lib. You can get the accelerator and magnetometer.
Fork of LSM303DLH by
LSM303DLH.cpp@3:9b4ff901b5c9, 2017-08-06 (annotated)
- Committer:
- salco
- Date:
- Sun Aug 06 00:43:57 2017 +0000
- Revision:
- 3:9b4ff901b5c9
- Parent:
- 2:aea5caec809c
- Child:
- 4:4f2ed3f8726c
lsm303dlh acceleration now work, but need clean up
Who changed what in which revision?
User | Revision | Line number | New 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 | |
shimniok | 0:de767f4959ef | 47 | enum REG_ADDRS { |
shimniok | 0:de767f4959ef | 48 | /* --- Mag --- */ |
shimniok | 0:de767f4959ef | 49 | CRA_REG_M = 0x00, |
shimniok | 0:de767f4959ef | 50 | CRB_REG_M = 0x01, |
shimniok | 0:de767f4959ef | 51 | MR_REG_M = 0x02, |
shimniok | 0:de767f4959ef | 52 | OUT_X_M = 0x03, |
shimniok | 0:de767f4959ef | 53 | OUT_Y_M = 0x05, |
shimniok | 0:de767f4959ef | 54 | OUT_Z_M = 0x07, |
shimniok | 0:de767f4959ef | 55 | /* --- Acc --- */ |
shimniok | 0:de767f4959ef | 56 | CTRL_REG1_A = 0x20, |
shimniok | 0:de767f4959ef | 57 | CTRL_REG4_A = 0x23, |
salco | 3:9b4ff901b5c9 | 58 | STATUS_REG_A= 0x27, |
shimniok | 0:de767f4959ef | 59 | OUT_X_A = 0x28, |
shimniok | 0:de767f4959ef | 60 | OUT_Y_A = 0x2A, |
shimniok | 0:de767f4959ef | 61 | OUT_Z_A = 0x2C, |
shimniok | 0:de767f4959ef | 62 | }; |
shimniok | 0:de767f4959ef | 63 | |
shimniok | 0:de767f4959ef | 64 | bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v) |
shimniok | 0:de767f4959ef | 65 | { |
salco | 3:9b4ff901b5c9 | 66 | bool result=false; |
salco | 3:9b4ff901b5c9 | 67 | char data[2] = {addr_reg, v}; |
salco | 3:9b4ff901b5c9 | 68 | //__disable_irq(); |
salco | 3:9b4ff901b5c9 | 69 | result = m_ptr_I2C->write(addr_i2c, data, 2) == 0; |
salco | 3:9b4ff901b5c9 | 70 | if(result == false) debug("Unable to Write \n"); |
salco | 3:9b4ff901b5c9 | 71 | |
salco | 3:9b4ff901b5c9 | 72 | //__enable_irq(); |
salco | 3:9b4ff901b5c9 | 73 | return result; |
shimniok | 0:de767f4959ef | 74 | } |
shimniok | 0:de767f4959ef | 75 | |
shimniok | 0:de767f4959ef | 76 | bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v) |
shimniok | 0:de767f4959ef | 77 | { |
shimniok | 0:de767f4959ef | 78 | char data = addr_reg; |
shimniok | 2:aea5caec809c | 79 | bool result = false; |
shimniok | 2:aea5caec809c | 80 | |
salco | 3:9b4ff901b5c9 | 81 | //__disable_irq(); |
salco | 3:9b4ff901b5c9 | 82 | if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0) |
salco | 3:9b4ff901b5c9 | 83 | { |
salco | 3:9b4ff901b5c9 | 84 | if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0) |
salco | 3:9b4ff901b5c9 | 85 | { |
salco | 3:9b4ff901b5c9 | 86 | *v = data; |
salco | 3:9b4ff901b5c9 | 87 | result = true; |
salco | 3:9b4ff901b5c9 | 88 | } |
salco | 3:9b4ff901b5c9 | 89 | else |
salco | 3:9b4ff901b5c9 | 90 | { |
salco | 3:9b4ff901b5c9 | 91 | debug("Unable to Read \n"); |
salco | 3:9b4ff901b5c9 | 92 | } |
shimniok | 0:de767f4959ef | 93 | } |
salco | 3:9b4ff901b5c9 | 94 | else |
salco | 3:9b4ff901b5c9 | 95 | { |
salco | 3:9b4ff901b5c9 | 96 | debug("Unable to Write \n"); |
salco | 3:9b4ff901b5c9 | 97 | } |
salco | 3:9b4ff901b5c9 | 98 | //__enable_irq(); |
shimniok | 2:aea5caec809c | 99 | return result; |
shimniok | 0:de767f4959ef | 100 | } |
shimniok | 0:de767f4959ef | 101 | |
shimniok | 0:de767f4959ef | 102 | bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v) |
shimniok | 0:de767f4959ef | 103 | { |
salco | 3:9b4ff901b5c9 | 104 | |
shimniok | 0:de767f4959ef | 105 | char *pv = (char *)v; |
shimniok | 2:aea5caec809c | 106 | bool result; |
shimniok | 2:aea5caec809c | 107 | |
shimniok | 2:aea5caec809c | 108 | result = read_reg(addr_i2c,addr_reg+0,pv+1); |
shimniok | 2:aea5caec809c | 109 | result &= read_reg(addr_i2c,addr_reg+1,pv+0); |
shimniok | 2:aea5caec809c | 110 | |
salco | 3:9b4ff901b5c9 | 111 | |
shimniok | 2:aea5caec809c | 112 | return result; |
shimniok | 0:de767f4959ef | 113 | } |
shimniok | 0:de767f4959ef | 114 | |
salco | 3:9b4ff901b5c9 | 115 | LSM303DLH::LSM303DLH(PinName sda, PinName scl) |
salco | 3:9b4ff901b5c9 | 116 | { |
salco | 3:9b4ff901b5c9 | 117 | m_ptr_I2C = new I2C(sda, scl); |
salco | 3:9b4ff901b5c9 | 118 | init(); |
salco | 3:9b4ff901b5c9 | 119 | } |
salco | 3:9b4ff901b5c9 | 120 | LSM303DLH::LSM303DLH(I2C* ptrI2C) |
salco | 3:9b4ff901b5c9 | 121 | { |
salco | 3:9b4ff901b5c9 | 122 | m_ptr_I2C = ptrI2C; |
salco | 3:9b4ff901b5c9 | 123 | //debug( "ptr pass %d, ptr here %d \n",(int)ptrI2C,(int)m_ptr_I2C); |
salco | 3:9b4ff901b5c9 | 124 | this->init(); |
salco | 3:9b4ff901b5c9 | 125 | } |
salco | 3:9b4ff901b5c9 | 126 | |
salco | 3:9b4ff901b5c9 | 127 | void LSM303DLH::init(void) |
shimniok | 0:de767f4959ef | 128 | { |
shimniok | 0:de767f4959ef | 129 | char reg_v; |
salco | 3:9b4ff901b5c9 | 130 | |
salco | 3:9b4ff901b5c9 | 131 | debug("In INIT \n"); |
salco | 3:9b4ff901b5c9 | 132 | _offset_x = 0; |
salco | 3:9b4ff901b5c9 | 133 | _offset_y = 0; |
salco | 3:9b4ff901b5c9 | 134 | _offset_z = 0; |
salco | 3:9b4ff901b5c9 | 135 | _scale_x = 0; |
salco | 3:9b4ff901b5c9 | 136 | _scale_y = 0; |
salco | 3:9b4ff901b5c9 | 137 | _scale_z = 0; |
salco | 3:9b4ff901b5c9 | 138 | _filt_ax = 0; |
salco | 3:9b4ff901b5c9 | 139 | _filt_ay = 0; |
salco | 3:9b4ff901b5c9 | 140 | _filt_az = 6000; |
salco | 3:9b4ff901b5c9 | 141 | |
salco | 3:9b4ff901b5c9 | 142 | |
salco | 3:9b4ff901b5c9 | 143 | m_ptr_I2C->frequency(100000); |
salco | 3:9b4ff901b5c9 | 144 | |
salco | 3:9b4ff901b5c9 | 145 | |
salco | 3:9b4ff901b5c9 | 146 | debug("in ACC \n"); |
shimniok | 0:de767f4959ef | 147 | reg_v = 0; |
shimniok | 0:de767f4959ef | 148 | reg_v |= 0x01 << 5; /* Normal mode */ |
shimniok | 0:de767f4959ef | 149 | reg_v |= 0x07; /* X/Y/Z axis enable. */ |
shimniok | 0:de767f4959ef | 150 | write_reg(addr_acc,CTRL_REG1_A,reg_v); |
shimniok | 0:de767f4959ef | 151 | reg_v = 0; |
shimniok | 0:de767f4959ef | 152 | read_reg(addr_acc,CTRL_REG1_A,®_v); |
shimniok | 0:de767f4959ef | 153 | |
shimniok | 0:de767f4959ef | 154 | reg_v = 0; |
salco | 3:9b4ff901b5c9 | 155 | reg_v |= 0x01 << 7; //full read |
salco | 3:9b4ff901b5c9 | 156 | reg_v |= 0x01 << 3; //hi res |
salco | 3:9b4ff901b5c9 | 157 | //reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ |
shimniok | 0:de767f4959ef | 158 | reg_v |= 0x01 << 4; /* +/- 4g */ |
shimniok | 0:de767f4959ef | 159 | write_reg(addr_acc,CTRL_REG4_A,reg_v); |
shimniok | 0:de767f4959ef | 160 | |
shimniok | 0:de767f4959ef | 161 | /* -- mag --- */ |
salco | 3:9b4ff901b5c9 | 162 | debug("in MAG \n"); |
shimniok | 0:de767f4959ef | 163 | reg_v = 0; |
shimniok | 0:de767f4959ef | 164 | reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ |
shimniok | 0:de767f4959ef | 165 | write_reg(addr_mag,CRA_REG_M,reg_v); |
shimniok | 0:de767f4959ef | 166 | |
shimniok | 0:de767f4959ef | 167 | reg_v = 0; |
shimniok | 0:de767f4959ef | 168 | //reg_v |= 0x01 << 5; /* +-1.3Gauss */ |
shimniok | 0:de767f4959ef | 169 | reg_v |= 0x07 << 5; /* +-8.1Gauss */ |
shimniok | 0:de767f4959ef | 170 | write_reg(addr_mag,CRB_REG_M,reg_v); |
shimniok | 0:de767f4959ef | 171 | |
shimniok | 0:de767f4959ef | 172 | reg_v = 0; /* Continuous-conversion mode */ |
shimniok | 0:de767f4959ef | 173 | write_reg(addr_mag,MR_REG_M,reg_v); |
salco | 3:9b4ff901b5c9 | 174 | |
salco | 3:9b4ff901b5c9 | 175 | debug("Out INIT \n"); |
salco | 3:9b4ff901b5c9 | 176 | clearscreen(); |
shimniok | 0:de767f4959ef | 177 | } |
shimniok | 0:de767f4959ef | 178 | |
shimniok | 0:de767f4959ef | 179 | void LSM303DLH::setOffset(float x, float y, float z) |
shimniok | 0:de767f4959ef | 180 | { |
shimniok | 0:de767f4959ef | 181 | _offset_x = x; |
shimniok | 0:de767f4959ef | 182 | _offset_y = y; |
shimniok | 0:de767f4959ef | 183 | _offset_z = z; |
shimniok | 0:de767f4959ef | 184 | } |
shimniok | 0:de767f4959ef | 185 | |
shimniok | 0:de767f4959ef | 186 | void LSM303DLH::setScale(float x, float y, float z) |
shimniok | 0:de767f4959ef | 187 | { |
shimniok | 0:de767f4959ef | 188 | _scale_x = x; |
shimniok | 0:de767f4959ef | 189 | _scale_y = y; |
shimniok | 0:de767f4959ef | 190 | _scale_z = z; |
shimniok | 0:de767f4959ef | 191 | } |
salco | 3:9b4ff901b5c9 | 192 | #define _FS 4 |
salco | 3:9b4ff901b5c9 | 193 | bool LSM303DLH::read(vector &a, vector &m) |
shimniok | 0:de767f4959ef | 194 | { |
salco | 3:9b4ff901b5c9 | 195 | |
salco | 3:9b4ff901b5c9 | 196 | bool result = true; |
shimniok | 0:de767f4959ef | 197 | short a_x, a_y, a_z; |
shimniok | 0:de767f4959ef | 198 | short m_x, m_y, m_z; |
salco | 3:9b4ff901b5c9 | 199 | #if defined(CHECK_TIME_SEQUENCE) |
salco | 3:9b4ff901b5c9 | 200 | Timer t; |
salco | 3:9b4ff901b5c9 | 201 | int usec1, usec2; |
salco | 3:9b4ff901b5c9 | 202 | |
salco | 3:9b4ff901b5c9 | 203 | t.reset(); |
salco | 3:9b4ff901b5c9 | 204 | t.start(); |
shimniok | 2:aea5caec809c | 205 | |
salco | 3:9b4ff901b5c9 | 206 | usec1 = t.read_us(); |
salco | 3:9b4ff901b5c9 | 207 | #endif |
salco | 3:9b4ff901b5c9 | 208 | |
salco | 3:9b4ff901b5c9 | 209 | union{ |
salco | 3:9b4ff901b5c9 | 210 | int16_t number; |
salco | 3:9b4ff901b5c9 | 211 | char byte[2]; |
salco | 3:9b4ff901b5c9 | 212 | struct{ |
salco | 3:9b4ff901b5c9 | 213 | char MSB; |
salco | 3:9b4ff901b5c9 | 214 | char LSB; |
salco | 3:9b4ff901b5c9 | 215 | }; |
salco | 3:9b4ff901b5c9 | 216 | }my_test; |
salco | 3:9b4ff901b5c9 | 217 | vector a_test; |
salco | 3:9b4ff901b5c9 | 218 | |
salco | 3:9b4ff901b5c9 | 219 | char data_read_acc =0; |
salco | 3:9b4ff901b5c9 | 220 | read_reg(addr_acc,STATUS_REG_A,&data_read_acc); |
salco | 3:9b4ff901b5c9 | 221 | if(data_read_acc & (1<<3))//new data |
salco | 3:9b4ff901b5c9 | 222 | { |
salco | 3:9b4ff901b5c9 | 223 | result &= read_reg(addr_acc,OUT_X_A ,&(my_test.MSB));//MSB at lower |
salco | 3:9b4ff901b5c9 | 224 | if(result) |
salco | 3:9b4ff901b5c9 | 225 | { |
salco | 3:9b4ff901b5c9 | 226 | result &= read_reg(addr_acc,OUT_X_A+1,&(my_test.LSB)); |
salco | 3:9b4ff901b5c9 | 227 | } |
salco | 3:9b4ff901b5c9 | 228 | if(result) |
salco | 3:9b4ff901b5c9 | 229 | { |
salco | 3:9b4ff901b5c9 | 230 | //a_test.x = my_test.number; |
salco | 3:9b4ff901b5c9 | 231 | a_test.x = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/)); |
salco | 3:9b4ff901b5c9 | 232 | //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 233 | setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 234 | } |
salco | 3:9b4ff901b5c9 | 235 | else |
salco | 3:9b4ff901b5c9 | 236 | { |
salco | 3:9b4ff901b5c9 | 237 | debug("error reading \n"); |
salco | 3:9b4ff901b5c9 | 238 | } |
salco | 3:9b4ff901b5c9 | 239 | |
salco | 3:9b4ff901b5c9 | 240 | if(result) |
salco | 3:9b4ff901b5c9 | 241 | { |
salco | 3:9b4ff901b5c9 | 242 | result &= read_reg(addr_acc,OUT_Y_A ,&(my_test.MSB));//MSB at lower |
salco | 3:9b4ff901b5c9 | 243 | } |
salco | 3:9b4ff901b5c9 | 244 | if(result) |
salco | 3:9b4ff901b5c9 | 245 | { |
salco | 3:9b4ff901b5c9 | 246 | result &= read_reg(addr_acc,OUT_Y_A+1,&(my_test.LSB)); |
salco | 3:9b4ff901b5c9 | 247 | } |
salco | 3:9b4ff901b5c9 | 248 | if(result) |
salco | 3:9b4ff901b5c9 | 249 | { |
salco | 3:9b4ff901b5c9 | 250 | //a_test.y = my_test.number; |
salco | 3:9b4ff901b5c9 | 251 | a_test.y = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/)); |
salco | 3:9b4ff901b5c9 | 252 | //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 253 | setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 254 | } |
salco | 3:9b4ff901b5c9 | 255 | else |
salco | 3:9b4ff901b5c9 | 256 | { |
salco | 3:9b4ff901b5c9 | 257 | debug("error reading \n"); |
salco | 3:9b4ff901b5c9 | 258 | } |
salco | 3:9b4ff901b5c9 | 259 | |
salco | 3:9b4ff901b5c9 | 260 | if(result) |
salco | 3:9b4ff901b5c9 | 261 | { |
salco | 3:9b4ff901b5c9 | 262 | result &= read_reg(addr_acc,OUT_Z_A ,&(my_test.MSB));//MSB at lower |
salco | 3:9b4ff901b5c9 | 263 | } |
salco | 3:9b4ff901b5c9 | 264 | if(result) |
salco | 3:9b4ff901b5c9 | 265 | { |
salco | 3:9b4ff901b5c9 | 266 | result &= read_reg(addr_acc,OUT_Z_A+1,&(my_test.LSB)); |
salco | 3:9b4ff901b5c9 | 267 | } |
salco | 3:9b4ff901b5c9 | 268 | if(result) |
salco | 3:9b4ff901b5c9 | 269 | { |
salco | 3:9b4ff901b5c9 | 270 | //a_test.z = my_test.number; |
salco | 3:9b4ff901b5c9 | 271 | a_test.z = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/)); |
salco | 3:9b4ff901b5c9 | 272 | //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 273 | setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]); |
salco | 3:9b4ff901b5c9 | 274 | } |
salco | 3:9b4ff901b5c9 | 275 | else |
salco | 3:9b4ff901b5c9 | 276 | { |
salco | 3:9b4ff901b5c9 | 277 | debug("error reading \n"); |
salco | 3:9b4ff901b5c9 | 278 | } |
salco | 3:9b4ff901b5c9 | 279 | //debug("test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z); |
salco | 3:9b4ff901b5c9 | 280 | setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z); |
salco | 3:9b4ff901b5c9 | 281 | } |
salco | 3:9b4ff901b5c9 | 282 | /*result &= read_reg_short(addr_acc, OUT_X_A, &a_x); |
salco | 3:9b4ff901b5c9 | 283 | result &= read_reg_short(addr_acc, OUT_Y_A, &a_y); |
salco | 3:9b4ff901b5c9 | 284 | result &= read_reg_short(addr_acc, OUT_Z_A, &a_z);*/ |
salco | 3:9b4ff901b5c9 | 285 | //This test pass so its ok |
shimniok | 0:de767f4959ef | 286 | |
salco | 3:9b4ff901b5c9 | 287 | result &= read_reg_short(addr_mag, OUT_X_M, &m_x); |
salco | 3:9b4ff901b5c9 | 288 | result &= read_reg_short(addr_mag, OUT_Y_M, &m_y); |
salco | 3:9b4ff901b5c9 | 289 | result &= read_reg_short(addr_mag, OUT_Z_M, &m_z); |
salco | 3:9b4ff901b5c9 | 290 | #if defined(CHECK_TIME_SEQUENCE) |
salco | 3:9b4ff901b5c9 | 291 | usec2 = t.read_us(); |
salco | 3:9b4ff901b5c9 | 292 | |
salco | 3:9b4ff901b5c9 | 293 | debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1); |
salco | 3:9b4ff901b5c9 | 294 | #endif |
salco | 3:9b4ff901b5c9 | 295 | if(result == true) |
salco | 3:9b4ff901b5c9 | 296 | { |
salco | 3:9b4ff901b5c9 | 297 | // Perform simple lowpass filtering |
salco | 3:9b4ff901b5c9 | 298 | // Intended to stabilize heading despite |
salco | 3:9b4ff901b5c9 | 299 | // device vibration such as on a UGV |
salco | 3:9b4ff901b5c9 | 300 | |
salco | 3:9b4ff901b5c9 | 301 | #if 0 |
salco | 3:9b4ff901b5c9 | 302 | if(data_read_acc & (1<<3)) |
salco | 3:9b4ff901b5c9 | 303 | { |
salco | 3:9b4ff901b5c9 | 304 | a_test.x= a_test.x / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/); |
salco | 3:9b4ff901b5c9 | 305 | a_test.y= a_test.y / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/); |
salco | 3:9b4ff901b5c9 | 306 | a_test.z= a_test.z / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/); |
salco | 3:9b4ff901b5c9 | 307 | |
salco | 3:9b4ff901b5c9 | 308 | debug("test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z); |
salco | 3:9b4ff901b5c9 | 309 | } |
salco | 3:9b4ff901b5c9 | 310 | #endif |
salco | 3:9b4ff901b5c9 | 311 | //float( a[i] ) * pow(2.,(fs+1)) / 32768. |
salco | 3:9b4ff901b5c9 | 312 | |
salco | 3:9b4ff901b5c9 | 313 | //x/8 = reading /0xFFFF(655355) |
salco | 3:9b4ff901b5c9 | 314 | |
salco | 3:9b4ff901b5c9 | 315 | |
salco | 3:9b4ff901b5c9 | 316 | |
salco | 3:9b4ff901b5c9 | 317 | // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT)); |
salco | 3:9b4ff901b5c9 | 318 | /*_filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT)); |
salco | 3:9b4ff901b5c9 | 319 | _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT); |
salco | 3:9b4ff901b5c9 | 320 | _filt_az += a_z - (_filt_az >> FILTER_SHIFT); |
salco | 3:9b4ff901b5c9 | 321 | |
salco | 3:9b4ff901b5c9 | 322 | |
salco | 3:9b4ff901b5c9 | 323 | |
salco | 3:9b4ff901b5c9 | 324 | a.x = (float) (_filt_ax >> FILTER_SHIFT); |
salco | 3:9b4ff901b5c9 | 325 | a.y = (float) (_filt_ay >> FILTER_SHIFT); |
salco | 3:9b4ff901b5c9 | 326 | a.z = (float) (_filt_az >> FILTER_SHIFT);*/ |
salco | 3:9b4ff901b5c9 | 327 | |
salco | 3:9b4ff901b5c9 | 328 | // offset and scale |
salco | 3:9b4ff901b5c9 | 329 | m.x = (m_x + _offset_x) * _scale_x; |
salco | 3:9b4ff901b5c9 | 330 | m.y = (m_y + _offset_y) * _scale_y; |
salco | 3:9b4ff901b5c9 | 331 | m.z = (m_z + _offset_z) * _scale_z; |
salco | 3:9b4ff901b5c9 | 332 | } |
salco | 3:9b4ff901b5c9 | 333 | |
salco | 3:9b4ff901b5c9 | 334 | return result; |
shimniok | 0:de767f4959ef | 335 | } |
shimniok | 0:de767f4959ef | 336 | |
shimniok | 0:de767f4959ef | 337 | |
shimniok | 0:de767f4959ef | 338 | // Returns the number of degrees from the -Y axis that it |
shimniok | 0:de767f4959ef | 339 | // is pointing. |
shimniok | 0:de767f4959ef | 340 | float LSM303DLH::heading() |
shimniok | 0:de767f4959ef | 341 | { |
shimniok | 0:de767f4959ef | 342 | return heading((vector){0,-1,0}); |
shimniok | 0:de767f4959ef | 343 | } |
shimniok | 0:de767f4959ef | 344 | |
shimniok | 0:de767f4959ef | 345 | float LSM303DLH::heading(vector from) |
shimniok | 0:de767f4959ef | 346 | { |
shimniok | 0:de767f4959ef | 347 | vector a, m; |
shimniok | 0:de767f4959ef | 348 | |
salco | 3:9b4ff901b5c9 | 349 | read(a, m); |
shimniok | 0:de767f4959ef | 350 | |
shimniok | 0:de767f4959ef | 351 | //////////////////////////////////////////////// |
shimniok | 0:de767f4959ef | 352 | // compute heading |
shimniok | 0:de767f4959ef | 353 | //////////////////////////////////////////////// |
shimniok | 0:de767f4959ef | 354 | |
shimniok | 0:de767f4959ef | 355 | vector temp_a = a; |
shimniok | 0:de767f4959ef | 356 | // normalize |
shimniok | 0:de767f4959ef | 357 | vector_normalize(&temp_a); |
shimniok | 0:de767f4959ef | 358 | //vector_normalize(&m); |
shimniok | 0:de767f4959ef | 359 | |
shimniok | 0:de767f4959ef | 360 | // compute E and N |
shimniok | 0:de767f4959ef | 361 | vector E; |
shimniok | 0:de767f4959ef | 362 | vector N; |
shimniok | 0:de767f4959ef | 363 | vector_cross(&m,&temp_a,&E); |
shimniok | 0:de767f4959ef | 364 | vector_normalize(&E); |
shimniok | 0:de767f4959ef | 365 | vector_cross(&temp_a,&E,&N); |
shimniok | 0:de767f4959ef | 366 | |
shimniok | 0:de767f4959ef | 367 | // compute heading |
shimniok | 0:de767f4959ef | 368 | float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI; |
shimniok | 0:de767f4959ef | 369 | if (heading < 0) heading += 360; |
shimniok | 0:de767f4959ef | 370 | |
shimniok | 0:de767f4959ef | 371 | return heading; |
shimniok | 0:de767f4959ef | 372 | } |
shimniok | 2:aea5caec809c | 373 | |
shimniok | 2:aea5caec809c | 374 | void LSM303DLH::frequency(int hz) |
shimniok | 2:aea5caec809c | 375 | { |
salco | 3:9b4ff901b5c9 | 376 | m_ptr_I2C->frequency(hz); |
salco | 3:9b4ff901b5c9 | 377 | } |