Hochschule München / LSM303D_imu

imu01c

LSM303D.cpp@2:4bfc36c368c2, 2016-02-09 (annotated)

Committer:
HMFK03LST1
Date:
Tue Feb 09 07:46:40 2016 +0000
Revision:
2:4bfc36c368c2
Parent:
1:5d0acf280330
Child:
3:a55edecc96e2
.

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 2:4bfc36c368c2 132
HMFK03LST1 2:4bfc36c368c2 133 min.x =-4500;
HMFK03LST1 2:4bfc36c368c2 134 min.y =-4500;
HMFK03LST1 2:4bfc36c368c2 135 min.z =-4500;
HMFK03LST1 2:4bfc36c368c2 136
HMFK03LST1 2:4bfc36c368c2 137 max.x = 1200;
HMFK03LST1 2:4bfc36c368c2 138 max.y = 1200;
HMFK03LST1 2:4bfc36c368c2 139 max.z = 100 ;
HMFK03LST1 0:456611adedf8 140 }
HMFK03LST1 0:456611adedf8 141
HMFK03LST1 0:456611adedf8 142
HMFK03LST1 0:456611adedf8 143 void LSM303D::setOffset(float x, float y, float z)
HMFK03LST1 0:456611adedf8 144 {
HMFK03LST1 1:5d0acf280330 145 _offset_x = x;
HMFK03LST1 1:5d0acf280330 146 _offset_y = y;
HMFK03LST1 1:5d0acf280330 147 _offset_z = z;
HMFK03LST1 0:456611adedf8 148 }
HMFK03LST1 0:456611adedf8 149
HMFK03LST1 0:456611adedf8 150 void LSM303D::setScale(float x, float y, float z)
HMFK03LST1 0:456611adedf8 151 {
HMFK03LST1 1:5d0acf280330 152 _scale_x = x;
HMFK03LST1 1:5d0acf280330 153 _scale_y = y;
HMFK03LST1 1:5d0acf280330 154 _scale_z = z;
HMFK03LST1 0:456611adedf8 155 }
HMFK03LST1 0:456611adedf8 156
HMFK03LST1 0:456611adedf8 157 void LSM303D::set_vectors()
HMFK03LST1 0:456611adedf8 158 {
HMFK03LST1 0:456611adedf8 159
HMFK03LST1 0:456611adedf8 160 // Perform simple lowpass filtering
HMFK03LST1 0:456611adedf8 161 // Intended to stabilize heading despite
HMFK03LST1 0:456611adedf8 162 // device vibration such as on a UGV
HMFK03LST1 0:456611adedf8 163 _filt_ax += acc_raw.x - (_filt_ax >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 164 _filt_ay += acc_raw.y - (_filt_ay >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 165 _filt_az += acc_raw.z - (_filt_az >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 166
HMFK03LST1 0:456611adedf8 167 acc.x = (float) (_filt_ax >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 168 acc.y = (float) (_filt_ay >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 169 acc.z = (float) (_filt_az >> FILTER_SHIFT);
HMFK03LST1 0:456611adedf8 170
HMFK03LST1 0:456611adedf8 171 // offset and scale
HMFK03LST1 2:4bfc36c368c2 172 //mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4;
HMFK03LST1 2:4bfc36c368c2 173 //mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4;
HMFK03LST1 2:4bfc36c368c2 174 //mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4;
HMFK03LST1 2:4bfc36c368c2 175 mag.x = (mag.x + ((mag_raw.x + (((max.x - min.x)/2) - max.x)) * (10000/(max.x - min.x))) )/2;
HMFK03LST1 2:4bfc36c368c2 176 mag.y = (mag.y + ((mag_raw.y + (((max.y - min.y)/2) - max.y)) * (10000/(max.y - min.y))) )/2;
HMFK03LST1 2:4bfc36c368c2 177 mag.z = (mag.z + ((mag_raw.z + (((max.z - min.z)/2) - max.z)) * (10000/(max.z - min.z))) )/2;
HMFK03LST1 2:4bfc36c368c2 178
HMFK03LST1 0:456611adedf8 179 vector_normalize(&mag);
HMFK03LST1 0:456611adedf8 180 }
HMFK03LST1 0:456611adedf8 181
HMFK03LST1 0:456611adedf8 182 void LSM303D::read()
HMFK03LST1 0:456611adedf8 183 {
HMFK03LST1 0:456611adedf8 184 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 2:4bfc36c368c2 185 char out[6] = {0,0,0,0,0,0};
HMFK03LST1 2:4bfc36c368c2 186
HMFK03LST1 0:456611adedf8 187 _compass.write( addr_ls303d, data,1);
HMFK03LST1 0:456611adedf8 188 _compass.read( addr_ls303d, out, 6);
HMFK03LST1 2:4bfc36c368c2 189
HMFK03LST1 0:456611adedf8 190 acc_raw.x= short( (((short)out[1]) << 8) | out[0] );
HMFK03LST1 0:456611adedf8 191 acc_raw.y= short( (((short)out[3]) << 8) | out[2] );
HMFK03LST1 0:456611adedf8 192 acc_raw.z= short( (((short)out[5]) << 8) | out[4] );
HMFK03LST1 0:456611adedf8 193
HMFK03LST1 0:456611adedf8 194 data[0] = (OUT_X_M | (1<<7));
HMFK03LST1 0:456611adedf8 195
HMFK03LST1 0:456611adedf8 196 _compass.write( addr_ls303d, data, 1 );
HMFK03LST1 0:456611adedf8 197 _compass.read ( addr_ls303d, out, 6 );
HMFK03LST1 0:456611adedf8 198
HMFK03LST1 0:456611adedf8 199 mag_raw.x= short( (((short)out[1]) << 8)| out[0]);
HMFK03LST1 0:456611adedf8 200 mag_raw.y= short( (((short)out[3]) << 8)| out[2]);
HMFK03LST1 0:456611adedf8 201 mag_raw.z= short( (((short)out[5]) << 8)| out[4]);
HMFK03LST1 2:4bfc36c368c2 202
HMFK03LST1 0:456611adedf8 203 set_vectors();
HMFK03LST1 0:456611adedf8 204
HMFK03LST1 0:456611adedf8 205 calc_pos();
HMFK03LST1 0:456611adedf8 206 }
HMFK03LST1 0:456611adedf8 207
HMFK03LST1 0:456611adedf8 208
HMFK03LST1 0:456611adedf8 209
HMFK03LST1 0:456611adedf8 210 void LSM303D::calc_pos(void)
HMFK03LST1 0:456611adedf8 211 {
HMFK03LST1 2:4bfc36c368c2 212 float x;
HMFK03LST1 2:4bfc36c368c2 213 float y;
HMFK03LST1 2:4bfc36c368c2 214 float c;
HMFK03LST1 0:456611adedf8 215 ////////////////////////////////////////////////
HMFK03LST1 0:456611adedf8 216 // compute heading
HMFK03LST1 0:456611adedf8 217 ////////////////////////////////////////////////
HMFK03LST1 0:456611adedf8 218 vector from = {1,0,0};
HMFK03LST1 0:456611adedf8 219 vector temp_a = acc;
HMFK03LST1 0:456611adedf8 220
HMFK03LST1 0:456611adedf8 221 // normalize
HMFK03LST1 0:456611adedf8 222 vector_normalize(&temp_a);
HMFK03LST1 0:456611adedf8 223
HMFK03LST1 0:456611adedf8 224 // compute E and N
HMFK03LST1 0:456611adedf8 225 vector E;
HMFK03LST1 0:456611adedf8 226 vector N;
HMFK03LST1 0:456611adedf8 227 vector_cross(&mag,&temp_a,&E);
HMFK03LST1 0:456611adedf8 228 vector_normalize(&E);
HMFK03LST1 0:456611adedf8 229 vector_cross(&temp_a,&E,&N);
HMFK03LST1 0:456611adedf8 230
HMFK03LST1 0:456611adedf8 231 // compute heading
HMFK03LST1 2:4bfc36c368c2 232 x = vector_dot(&E,&from);
HMFK03LST1 2:4bfc36c368c2 233 y = vector_dot(&N,&from);
HMFK03LST1 2:4bfc36c368c2 234 c = sqrt(x*x + y*y);
HMFK03LST1 2:4bfc36c368c2 235 //hdg = atan2(x, y)) * 180/M_PI;
HMFK03LST1 2:4bfc36c368c2 236 hdg = atan2(x, y); // * 180/M_PI;
HMFK03LST1 2:4bfc36c368c2 237 hdg = ((asin(fabs(x)/c) * (1 - fabs(sin(hdg)))) + (acos(fabs(y)/c) * fabs(sin(hdg))))* 180 / M_PI;
HMFK03LST1 2:4bfc36c368c2 238 if ((x > 0) && (y < 0)) hdg = 180 - hdg;
HMFK03LST1 2:4bfc36c368c2 239 if ((x < 0) && (y < 0)) hdg = 180 + hdg;
HMFK03LST1 2:4bfc36c368c2 240 if ((x < 0) && (y > 0)) hdg = 360 - hdg;
HMFK03LST1 2:4bfc36c368c2 241
HMFK03LST1 2:4bfc36c368c2 242 //if (hdg < 0) hdg += 360;
HMFK03LST1 2:4bfc36c368c2 243 //if (hdg > 360) hdg -= 360;
HMFK03LST1 0:456611adedf8 244
HMFK03LST1 0:456611adedf8 245 vector_norm_xz(&temp_a);
HMFK03LST1 2:4bfc36c368c2 246 pitch = asin(temp_a.x) * 180/M_PI + 25;
HMFK03LST1 0:456611adedf8 247
HMFK03LST1 0:456611adedf8 248 }
HMFK03LST1 0:456611adedf8 249
HMFK03LST1 2:4bfc36c368c2 250 void LSM303D::set_limits(void)
HMFK03LST1 2:4bfc36c368c2 251 {
HMFK03LST1 2:4bfc36c368c2 252 if (max.x < mag_raw.x) max.x = mag_raw.x;
HMFK03LST1 2:4bfc36c368c2 253 if (min.x > mag_raw.x) min.x = mag_raw.x;
HMFK03LST1 2:4bfc36c368c2 254 if (max.y < mag_raw.y) max.y = mag_raw.y;
HMFK03LST1 2:4bfc36c368c2 255 if (min.y > mag_raw.y) min.y = mag_raw.y;
HMFK03LST1 2:4bfc36c368c2 256 if (max.z < mag_raw.z) max.z = mag_raw.z;
HMFK03LST1 2:4bfc36c368c2 257 if (min.z > mag_raw.z) min.z = mag_raw.z;
HMFK03LST1 2:4bfc36c368c2 258
HMFK03LST1 2:4bfc36c368c2 259
HMFK03LST1 2:4bfc36c368c2 260 if ((max.x - min.x) > 7200) {max.x--; min.x++;};
HMFK03LST1 2:4bfc36c368c2 261 if ((max.y - min.y) > 7200) {max.y--; min.y++;};
HMFK03LST1 2:4bfc36c368c2 262 if ((max.z - min.z) > 6200) {max.z--; min.z++;};
HMFK03LST1 2:4bfc36c368c2 263 }
HMFK03LST1 2:4bfc36c368c2 264
HMFK03LST1 0:456611adedf8 265 void LSM303D::frequency(int hz)
HMFK03LST1 0:456611adedf8 266 {
HMFK03LST1 0:456611adedf8 267 _compass.frequency(hz);
HMFK03LST1 0:456611adedf8 268 }