imu01c

Committer:
HMFK03LST1
Date:
Wed Jul 20 14:13:23 2016 +0000
Revision:
3:a55edecc96e2
Parent:
2:4bfc36c368c2
first

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