Hochschule München / LSM303D_imu

imu01c

LSM303D.cpp@1:5d0acf280330, 2015-10-31 (annotated)

Committer:
HMFK03LST1
Date:
Sat Oct 31 16:28:12 2015 +0000
Revision:
1:5d0acf280330
Parent:
0:456611adedf8
Child:
2:4bfc36c368c2
stable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HMFK03LST1 0:456611adedf8 1 /** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter
HMFK03LST1 0:456611adedf8 2 *
HMFK03LST1 0:456611adedf8 3 * Based on
HMFK03LST1 0:456611adedf8 4 *
HMFK03LST1 0:456611adedf8 5 * Michael Shimniok http://bot-thoughts.com
HMFK03LST1 0:456611adedf8 6 *
HMFK03LST1 0:456611adedf8 7 * test program by tosihisa and
HMFK03LST1 0:456611adedf8 8 *
HMFK03LST1 0:456611adedf8 9 * Pololu sample library for LSM303DLH breakout by ryantm:
HMFK03LST1 0:456611adedf8 10 *
HMFK03LST1 0:456611adedf8 11 * Copyright (c) 2011 Pololu Corporation. For more information, see
HMFK03LST1 0:456611adedf8 12 *
HMFK03LST1 0:456611adedf8 13 * http://www.pololu.com/
HMFK03LST1 0:456611adedf8 14 * http://forum.pololu.com/
HMFK03LST1 0:456611adedf8 15 *
HMFK03LST1 0:456611adedf8 16 * Permission is hereby granted, free of charge, to any person
HMFK03LST1 0:456611adedf8 17 * obtaining a copy of this software and associated documentation
HMFK03LST1 0:456611adedf8 18 * files (the "Software"), to deal in the Software without
HMFK03LST1 0:456611adedf8 19 * restriction, including without limitation the rights to use,
HMFK03LST1 0:456611adedf8 20 * copy, modify, merge, publish, distribute, sublicense, and/or sell
HMFK03LST1 0:456611adedf8 21 * copies of the Software, and to permit persons to whom the
HMFK03LST1 0:456611adedf8 22 * Software is furnished to do so, subject to the following
HMFK03LST1 0:456611adedf8 23 * conditions:
HMFK03LST1 0:456611adedf8 24 *
HMFK03LST1 0:456611adedf8 25 * The above copyright notice and this permission notice shall be
HMFK03LST1 0:456611adedf8 26 * included in all copies or substantial portions of the Software.
HMFK03LST1 0:456611adedf8 27 *
HMFK03LST1 0:456611adedf8 28 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
HMFK03LST1 0:456611adedf8 29 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
HMFK03LST1 0:456611adedf8 30 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
HMFK03LST1 0:456611adedf8 31 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HMFK03LST1 0:456611adedf8 32 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
HMFK03LST1 0:456611adedf8 33 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
HMFK03LST1 0:456611adedf8 34 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
HMFK03LST1 0:456611adedf8 35 * OTHER DEALINGS IN THE SOFTWARE.
HMFK03LST1 0:456611adedf8 36 */
HMFK03LST1 0:456611adedf8 37
HMFK03LST1 0:456611adedf8 38 #include "mbed.h"
HMFK03LST1 0:456611adedf8 39 #include "LSM303D.h"
HMFK03LST1 0:456611adedf8 40
HMFK03LST1 0:456611adedf8 41 #ifndef M_PI
HMFK03LST1 0:456611adedf8 42 #define M_PI 3.14159265358979323846
HMFK03LST1 0:456611adedf8 43 #endif
HMFK03LST1 0:456611adedf8 44
HMFK03LST1 0:456611adedf8 45 #define FILTER_SHIFT 2 // used in filtering acceleromter readings
HMFK03LST1 0:456611adedf8 46
HMFK03LST1 0:456611adedf8 47 const int addr_ls303d = 0x3A;
HMFK03LST1 0:456611adedf8 48
HMFK03LST1 0:456611adedf8 49 enum REG_ADDRS {
HMFK03LST1 0:456611adedf8 50 /* --- Mag --- */
HMFK03LST1 0:456611adedf8 51 OUT_TEMP = 0x05,
HMFK03LST1 0:456611adedf8 52 OUT_X_M = 0x08,
HMFK03LST1 0:456611adedf8 53 OUT_Y_M = 0x0A,
HMFK03LST1 0:456611adedf8 54 OUT_Z_M = 0x0C,
HMFK03LST1 0:456611adedf8 55 /* --- Acc --- */
HMFK03LST1 0:456611adedf8 56 CTRL_REG1_A = 0x20,
HMFK03LST1 0:456611adedf8 57 CTRL_REG2_A = 0x21,
HMFK03LST1 0:456611adedf8 58 CTRL_REG5_A = 0x24,
HMFK03LST1 0:456611adedf8 59 CTRL_REG6_A = 0x25,
HMFK03LST1 0:456611adedf8 60 CTRL_REG7_A = 0x26,
HMFK03LST1 0:456611adedf8 61 OUT_X_A = 0x28,
HMFK03LST1 0:456611adedf8 62 OUT_Y_A = 0x2A,
HMFK03LST1 0:456611adedf8 63 OUT_Z_A = 0x2C,
HMFK03LST1 0:456611adedf8 64 };
HMFK03LST1 0:456611adedf8 65
HMFK03LST1 0:456611adedf8 66 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
HMFK03LST1 0:456611adedf8 67 {
HMFK03LST1 0:456611adedf8 68 char data[2] = {addr_reg, v};
HMFK03LST1 0:456611adedf8 69 return LSM303D::_compass.write(addr_i2c, data, 2) == 0;
HMFK03LST1 0:456611adedf8 70 }
HMFK03LST1 0:456611adedf8 71
HMFK03LST1 0:456611adedf8 72 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
HMFK03LST1 0:456611adedf8 73 {
HMFK03LST1 0:456611adedf8 74 char data = addr_reg;
HMFK03LST1 0:456611adedf8 75 bool result = false;
HMFK03LST1 0:456611adedf8 76
HMFK03LST1 0:456611adedf8 77 __disable_irq();
HMFK03LST1 0:456611adedf8 78 if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
HMFK03LST1 0:456611adedf8 79 *v = data;
HMFK03LST1 0:456611adedf8 80 result = true;
HMFK03LST1 0:456611adedf8 81 }
HMFK03LST1 0:456611adedf8 82 __enable_irq();
HMFK03LST1 0:456611adedf8 83 return result;
HMFK03LST1 0:456611adedf8 84 }
HMFK03LST1 0:456611adedf8 85
HMFK03LST1 0:456611adedf8 86
HMFK03LST1 0:456611adedf8 87
HMFK03LST1 0:456611adedf8 88 LSM303D::LSM303D(PinName sda, PinName scl):
HMFK03LST1 0:456611adedf8 89 _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(1), _scale_y(1), _scale_z(1), _filt_ax(0), _filt_ay(0), _filt_az(9000)
HMFK03LST1 0:456611adedf8 90 {
HMFK03LST1 0:456611adedf8 91 char reg_v;
HMFK03LST1 0:456611adedf8 92 //_compass.frequency(100000);
HMFK03LST1 0:456611adedf8 93
HMFK03LST1 0:456611adedf8 94 //Chip Setup
HMFK03LST1 0:456611adedf8 95
HMFK03LST1 0:456611adedf8 96 //CTRL 0 (1Fh)all Default
HMFK03LST1 0:456611adedf8 97
HMFK03LST1 0:456611adedf8 98 //CTRL 1 (20h)
HMFK03LST1 0:456611adedf8 99 reg_v = 0;
HMFK03LST1 0:456611adedf8 100 reg_v |= 0x04 << 4; /* ACC 25Hz Autosample */
HMFK03LST1 0:456611adedf8 101 reg_v |= 0x07; /* X/Y/Z axis enable. */
HMFK03LST1 0:456611adedf8 102 write_reg(addr_ls303d,CTRL_REG1_A,reg_v); //
HMFK03LST1 0:456611adedf8 103 reg_v = 0;
HMFK03LST1 0:456611adedf8 104 read_reg(addr_ls303d,CTRL_REG1_A,&reg_v);
HMFK03LST1 0:456611adedf8 105
HMFK03LST1 0:456611adedf8 106 //CTRL 2 (21h)
HMFK03LST1 0:456611adedf8 107 reg_v = 0x00;
HMFK03LST1 0:456611adedf8 108 reg_v |= 0x03 << 6; /* 50Hz Antialias Filter */
HMFK03LST1 0:456611adedf8 109 reg_v |= 0x00 << 3; /* +/- 2g */
HMFK03LST1 0:456611adedf8 110 write_reg(addr_ls303d,CTRL_REG2_A,reg_v);
HMFK03LST1 0:456611adedf8 111
HMFK03LST1 0:456611adedf8 112 //CTRL 3 (22h) all Default
HMFK03LST1 0:456611adedf8 113
HMFK03LST1 0:456611adedf8 114 //CTRL 4 (23h) all Default
HMFK03LST1 0:456611adedf8 115
HMFK03LST1 0:456611adedf8 116 //CTRL 5 (24h)
HMFK03LST1 0:456611adedf8 117 reg_v = 0x00;
HMFK03LST1 0:456611adedf8 118 reg_v |= 0x01 << 7; /* Temp Sensor Enable 1*/
HMFK03LST1 0:456611adedf8 119 reg_v |= 0x03 << 5; /* Mag high Res 3*/
HMFK03LST1 0:456611adedf8 120 reg_v |= 0x03 << 2; /* Mag 25Hz Autosample*/
HMFK03LST1 0:456611adedf8 121 write_reg(addr_ls303d,CTRL_REG5_A,reg_v);
HMFK03LST1 0:456611adedf8 122
HMFK03LST1 0:456611adedf8 123 //CTRL 6 (25h)
HMFK03LST1 0:456611adedf8 124 reg_v = 0x00;
HMFK03LST1 0:456611adedf8 125 reg_v |= 0x00 << 5; /* +-2 Gauss*/
HMFK03LST1 0:456611adedf8 126 write_reg(addr_ls303d,CTRL_REG6_A,reg_v); //25h
HMFK03LST1 0:456611adedf8 127
HMFK03LST1 0:456611adedf8 128 //CTRL 7 (26h)
HMFK03LST1 0:456611adedf8 129 reg_v = 0x00;
HMFK03LST1 0:456611adedf8 130 reg_v |= 0x00 << 0; /* Mag Continuous Conversation*/
HMFK03LST1 0:456611adedf8 131 write_reg(addr_ls303d,CTRL_REG7_A,reg_v); //26h
HMFK03LST1 0:456611adedf8 132 }
HMFK03LST1 0:456611adedf8 133
HMFK03LST1 0:456611adedf8 134
HMFK03LST1 0:456611adedf8 135 void LSM303D::setOffset(float x, float y, float z)
HMFK03LST1 0:456611adedf8 136 {
HMFK03LST1 1:5d0acf280330 137 _offset_x = x;
HMFK03LST1 1:5d0acf280330 138 _offset_y = y;
HMFK03LST1 1:5d0acf280330 139 _offset_z = z;
HMFK03LST1 0:456611adedf8 140 }
HMFK03LST1 0:456611adedf8 141
HMFK03LST1 0:456611adedf8 142 void LSM303D::setScale(float x, float y, float z)
HMFK03LST1 0:456611adedf8 143 {
HMFK03LST1 1:5d0acf280330 144 _scale_x = x;
HMFK03LST1 1:5d0acf280330 145 _scale_y = y;
HMFK03LST1 1:5d0acf280330 146 _scale_z = z;
HMFK03LST1 0:456611adedf8 147 }
HMFK03LST1 0:456611adedf8 148
HMFK03LST1 0:456611adedf8 149 void LSM303D::set_vectors()
HMFK03LST1 0:456611adedf8 150 {
HMFK03LST1 0:456611adedf8 151
HMFK03LST1 0:456611adedf8 152 // Perform simple lowpass filtering
HMFK03LST1 0:456611adedf8 153 // Intended to stabilize heading despite
HMFK03LST1 0:456611adedf8 154 // device vibration such as on a UGV
HMFK03LST1 0:456611adedf8 155 _filt_ax += acc_raw.x - (_filt_ax >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 156 _filt_ay += acc_raw.y - (_filt_ay >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 157 _filt_az += acc_raw.z - (_filt_az >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 158
HMFK03LST1 0:456611adedf8 159 acc.x = (float) (_filt_ax >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 160 acc.y = (float) (_filt_ay >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 161 acc.z = (float) (_filt_az >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 162
HMFK03LST1 0:456611adedf8 163 // offset and scale
HMFK03LST1 1:5d0acf280330 164 mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4;
HMFK03LST1 1:5d0acf280330 165 mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4;
HMFK03LST1 1:5d0acf280330 166 mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4;
HMFK03LST1 0:456611adedf8 167
HMFK03LST1 0:456611adedf8 168 vector_normalize(&mag);
HMFK03LST1 0:456611adedf8 169 }
HMFK03LST1 0:456611adedf8 170
HMFK03LST1 0:456611adedf8 171 void LSM303D::read()
HMFK03LST1 0:456611adedf8 172 {
HMFK03LST1 0:456611adedf8 173 char data[1] = { OUT_X_A | (1<<7)}; //Page 23 In order to read multiple bytes, it is necessary to assert the most significant bit of the subaddress field. In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address of the first register to be read.
HMFK03LST1 0:456611adedf8 174 char out[6] = {0,0,0,0,0,0};
HMFK03LST1 0:456611adedf8 175 _compass.write( addr_ls303d, data,1);
HMFK03LST1 0:456611adedf8 176 _compass.read( addr_ls303d, out, 6);
HMFK03LST1 0:456611adedf8 177
HMFK03LST1 0:456611adedf8 178 acc_raw.x= short( (((short)out[1]) << 8) | out[0] );
HMFK03LST1 0:456611adedf8 179 acc_raw.y= short( (((short)out[3]) << 8) | out[2] );
HMFK03LST1 0:456611adedf8 180 acc_raw.z= short( (((short)out[5]) << 8) | out[4] );
HMFK03LST1 0:456611adedf8 181
HMFK03LST1 0:456611adedf8 182 data[0] = (OUT_X_M | (1<<7));
HMFK03LST1 0:456611adedf8 183
HMFK03LST1 0:456611adedf8 184 _compass.write( addr_ls303d, data, 1 );
HMFK03LST1 0:456611adedf8 185 _compass.read ( addr_ls303d, out, 6 );
HMFK03LST1 0:456611adedf8 186
HMFK03LST1 0:456611adedf8 187 mag_raw.x= short( (((short)out[1]) << 8)| out[0]);
HMFK03LST1 0:456611adedf8 188 mag_raw.y= short( (((short)out[3]) << 8)| out[2]);
HMFK03LST1 0:456611adedf8 189 mag_raw.z= short( (((short)out[5]) << 8)| out[4]);
HMFK03LST1 0:456611adedf8 190
HMFK03LST1 0:456611adedf8 191 set_vectors();
HMFK03LST1 0:456611adedf8 192
HMFK03LST1 0:456611adedf8 193 calc_pos();
HMFK03LST1 0:456611adedf8 194 }
HMFK03LST1 0:456611adedf8 195
HMFK03LST1 0:456611adedf8 196
HMFK03LST1 0:456611adedf8 197
HMFK03LST1 0:456611adedf8 198 void LSM303D::calc_pos(void)
HMFK03LST1 0:456611adedf8 199 {
HMFK03LST1 0:456611adedf8 200 ////////////////////////////////////////////////
HMFK03LST1 0:456611adedf8 201 // compute heading
HMFK03LST1 0:456611adedf8 202 ////////////////////////////////////////////////
HMFK03LST1 0:456611adedf8 203 vector from = {1,0,0};
HMFK03LST1 0:456611adedf8 204 vector temp_a = acc;
HMFK03LST1 0:456611adedf8 205
HMFK03LST1 0:456611adedf8 206 // normalize
HMFK03LST1 0:456611adedf8 207 vector_normalize(&temp_a);
HMFK03LST1 0:456611adedf8 208
HMFK03LST1 0:456611adedf8 209 // compute E and N
HMFK03LST1 0:456611adedf8 210 vector E;
HMFK03LST1 0:456611adedf8 211 vector N;
HMFK03LST1 0:456611adedf8 212 vector_cross(&mag,&temp_a,&E);
HMFK03LST1 0:456611adedf8 213 vector_normalize(&E);
HMFK03LST1 0:456611adedf8 214 vector_cross(&temp_a,&E,&N);
HMFK03LST1 0:456611adedf8 215
HMFK03LST1 0:456611adedf8 216 // compute heading
HMFK03LST1 0:456611adedf8 217 hdg = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
HMFK03LST1 0:456611adedf8 218 if (hdg < 0) hdg += 360;
HMFK03LST1 0:456611adedf8 219
HMFK03LST1 0:456611adedf8 220 vector_norm_xz(&temp_a);
HMFK03LST1 0:456611adedf8 221 pitch = asin(-temp_a.x) * 180/M_PI + 30;
HMFK03LST1 0:456611adedf8 222
HMFK03LST1 0:456611adedf8 223 }
HMFK03LST1 0:456611adedf8 224
HMFK03LST1 0:456611adedf8 225 void LSM303D::frequency(int hz)
HMFK03LST1 0:456611adedf8 226 {
HMFK03LST1 0:456611adedf8 227 _compass.frequency(hz);
HMFK03LST1 0:456611adedf8 228 }