imu01c
Embed:
(wiki syntax)
Show/hide line numbers
LSM303D.cpp
00001 /** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter 00002 * 00003 * Based on 00004 * 00005 * Michael Shimniok http://bot-thoughts.com 00006 * 00007 * test program by tosihisa and 00008 * 00009 * Pololu sample library for LSM303DLH breakout by ryantm: 00010 * 00011 * Copyright (c) 2011 Pololu Corporation. For more information, see 00012 * 00013 * http://www.pololu.com/ 00014 * http://forum.pololu.com/ 00015 * 00016 * Permission is hereby granted, free of charge, to any person 00017 * obtaining a copy of this software and associated documentation 00018 * files (the "Software"), to deal in the Software without 00019 * restriction, including without limitation the rights to use, 00020 * copy, modify, merge, publish, distribute, sublicense, and/or sell 00021 * copies of the Software, and to permit persons to whom the 00022 * Software is furnished to do so, subject to the following 00023 * conditions: 00024 * 00025 * The above copyright notice and this permission notice shall be 00026 * included in all copies or substantial portions of the Software. 00027 * 00028 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 00029 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 00030 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 00031 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 00032 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 00033 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 00034 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 00035 * OTHER DEALINGS IN THE SOFTWARE. 00036 */ 00037 00038 #include "mbed.h" 00039 #include "LSM303D.h" 00040 00041 #ifndef M_PI 00042 #define M_PI 3.14159265358979323846 00043 #endif 00044 00045 #define FILTER_SHIFT 1 // used in filtering acceleromter readings 00046 00047 const int addr_ls303d = 0x3A; 00048 00049 enum REG_ADDRS { 00050 /* --- Mag --- */ 00051 OUT_TEMP = 0x05, 00052 OUT_X_M = 0x08, 00053 OUT_Y_M = 0x0A, 00054 OUT_Z_M = 0x0C, 00055 /* --- Acc --- */ 00056 CTRL_REG1_A = 0x20, 00057 CTRL_REG2_A = 0x21, 00058 CTRL_REG5_A = 0x24, 00059 CTRL_REG6_A = 0x25, 00060 CTRL_REG7_A = 0x26, 00061 OUT_X_A = 0x28, 00062 OUT_Y_A = 0x2A, 00063 OUT_Z_A = 0x2C, 00064 }; 00065 00066 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v) 00067 { 00068 char data[2] = {addr_reg, v}; 00069 return LSM303D::_compass.write(addr_i2c, data, 2) == 0; 00070 } 00071 00072 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v) 00073 { 00074 char data = addr_reg; 00075 bool result = false; 00076 00077 __disable_irq(); 00078 if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){ 00079 *v = data; 00080 result = true; 00081 } 00082 __enable_irq(); 00083 return result; 00084 } 00085 00086 00087 00088 LSM303D::LSM303D(PinName sda, PinName scl): 00089 _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) 00090 { 00091 char reg_v; 00092 //_compass.frequency(100000); 00093 00094 //Chip Setup 00095 00096 //CTRL 0 (1Fh)all Default 00097 00098 //CTRL 1 (20h) 00099 reg_v = 0; 00100 reg_v |= 0x04 << 4; /* ACC 25Hz Autosample */ 00101 reg_v |= 0x07; /* X/Y/Z axis enable. */ 00102 write_reg(addr_ls303d,CTRL_REG1_A,reg_v); // 00103 reg_v = 0; 00104 read_reg(addr_ls303d,CTRL_REG1_A,®_v); 00105 00106 //CTRL 2 (21h) 00107 reg_v = 0x00; 00108 reg_v |= 0x03 << 6; /* 50Hz Antialias Filter */ 00109 reg_v |= 0x00 << 3; /* +/- 2g */ 00110 write_reg(addr_ls303d,CTRL_REG2_A,reg_v); 00111 00112 //CTRL 3 (22h) all Default 00113 00114 //CTRL 4 (23h) all Default 00115 00116 //CTRL 5 (24h) 00117 reg_v = 0x00; 00118 reg_v |= 0x01 << 7; /* Temp Sensor Enable 1*/ 00119 reg_v |= 0x03 << 5; /* Mag high Res 3*/ 00120 reg_v |= 0x03 << 2; /* Mag 25Hz Autosample*/ 00121 write_reg(addr_ls303d,CTRL_REG5_A,reg_v); 00122 00123 //CTRL 6 (25h) 00124 reg_v = 0x00; 00125 reg_v |= 0x00 << 5; /* +-2 Gauss*/ 00126 write_reg(addr_ls303d,CTRL_REG6_A,reg_v); //25h 00127 00128 //CTRL 7 (26h) 00129 reg_v = 0x00; 00130 reg_v |= 0x00 << 0; /* Mag Continuous Conversation*/ 00131 write_reg(addr_ls303d,CTRL_REG7_A,reg_v); //26h 00132 00133 min.x =-2000; 00134 min.y =-2000; 00135 min.z = 1000; 00136 00137 max.x = -1000; 00138 max.y = -1000; 00139 max.z = 4000; 00140 00141 spreed.x = 6000; 00142 spreed.y = 6000; 00143 spreed.z = 6000; 00144 } 00145 00146 00147 void LSM303D::setOffset(float x, float y, float z) 00148 { 00149 _offset_x = x; 00150 _offset_y = y; 00151 _offset_z = z; 00152 } 00153 00154 void LSM303D::setScale(float x, float y, float z) 00155 { 00156 _scale_x = x; 00157 _scale_y = y; 00158 _scale_z = z; 00159 } 00160 00161 void LSM303D::set_vectors() 00162 { 00163 00164 // Perform simple lowpass filtering 00165 // Intended to stabilize heading despite 00166 // device vibration such as on a UGV 00167 _filt_ax += acc_raw.x - (_filt_ax >> FILTER_SHIFT); 00168 _filt_ay += acc_raw.y - (_filt_ay >> FILTER_SHIFT); 00169 _filt_az += acc_raw.z - (_filt_az >> FILTER_SHIFT); 00170 00171 acc.x = (float) (_filt_ax >> FILTER_SHIFT); 00172 acc.y = (float) (_filt_ay >> FILTER_SHIFT); 00173 acc.z = (float) (_filt_az >> FILTER_SHIFT); 00174 00175 // offset and scale 00176 //mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4; 00177 //mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4; 00178 //mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4; 00179 mag.x = (mag.x + ((mag_raw.x + (((max.x - min.x)/2) - max.x)) * (10000/(max.x - min.x))) )/2; 00180 mag.y = (mag.y + ((mag_raw.y + (((max.y - min.y)/2) - max.y)) * (10000/(max.y - min.y))) )/2; 00181 mag.z = (mag.z + ((mag_raw.z + (((max.z - min.z)/2) - max.z)) * (10000/(max.z - min.z))) )/2; 00182 00183 vector_normalize(&mag); 00184 } 00185 00186 void LSM303D::read() 00187 { 00188 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. 00189 char out[6] = {0,0,0,0,0,0}; 00190 00191 _compass.write( addr_ls303d, data,1); 00192 _compass.read( addr_ls303d, out, 6); 00193 00194 acc_raw.x= short( (((short)out[1]) << 8) | out[0] ); 00195 acc_raw.y= short( (((short)out[3]) << 8) | out[2] ); 00196 acc_raw.z= short( (((short)out[5]) << 8) | out[4] ); 00197 00198 data[0] = (OUT_X_M | (1<<7)); 00199 00200 _compass.write( addr_ls303d, data, 1 ); 00201 _compass.read ( addr_ls303d, out, 6 ); 00202 00203 mag_raw.x= short( (((short)out[1]) << 8)| out[0]); 00204 mag_raw.y= short( (((short)out[3]) << 8)| out[2]); 00205 mag_raw.z= short( (((short)out[5]) << 8)| out[4]); 00206 00207 set_vectors(); 00208 00209 calc_pos(); 00210 } 00211 00212 00213 00214 void LSM303D::calc_pos(void) 00215 { 00216 float x; 00217 float y; 00218 float c; 00219 //////////////////////////////////////////////// 00220 // compute heading 00221 //////////////////////////////////////////////// 00222 vector from = {1,0,0}; 00223 vector temp_a = acc; 00224 00225 // normalize 00226 vector_normalize(&temp_a); 00227 00228 // compute E and N 00229 vector E; //vector East 00230 vector N; //vector Nord 00231 vector_cross(&mag,&temp_a,&E); 00232 vector_normalize(&E); 00233 vector_cross(&temp_a,&E,&N); 00234 00235 // compute heading 00236 x = vector_dot(&E,&from); 00237 y = vector_dot(&N,&from); 00238 c = sqrt(x*x + y*y); 00239 //hdg = atan2(x, y)) * 180/M_PI; 00240 hdg = atan2(x, y); // * 180/M_PI; 00241 hdg = ((asin(fabs(x)/c) * (1 - fabs(sin(hdg)))) + (acos(fabs(y)/c) * fabs(sin(hdg))))* 180 / M_PI; 00242 if ((x > 0) && (y < 0)) hdg = 180 - hdg; 00243 if ((x < 0) && (y < 0)) hdg = 180 + hdg; 00244 if ((x < 0) && (y > 0)) hdg = 360 - hdg; 00245 00246 //if (hdg < 0) hdg += 360; 00247 //if (hdg > 360) hdg -= 360; 00248 00249 vector_norm_xz(&temp_a); 00250 pitch = (asin(temp_a.x) * 180/M_PI) + 25; 00251 00252 } 00253 00254 void LSM303D::set_limits(int mode) 00255 { 00256 if(mode == 1) //first cal. 00257 { 00258 if (max.x < mag_raw.x) max.x = mag_raw.x; 00259 if (min.x > mag_raw.x) min.x = mag_raw.x; 00260 if (max.y < mag_raw.y) max.y = mag_raw.y; 00261 if (min.y > mag_raw.y) min.y = mag_raw.y; 00262 if (max.z < mag_raw.z) max.z = mag_raw.z; 00263 if (min.z > mag_raw.z) min.z = mag_raw.z; 00264 } 00265 if(mode == 2) //autorecal. 00266 { 00267 if (max.x < mag_raw.x) max.x += 5; 00268 if (min.x > mag_raw.x) min.x -= 5; 00269 if (max.y < mag_raw.y) max.y += 5; 00270 if (min.y > mag_raw.y) min.y -= 5; 00271 if (max.z < mag_raw.z) max.z += 5; 00272 if (min.z > mag_raw.z) min.z -= 5; 00273 } 00274 00275 if ((max.x - min.x) > spreed.x) {max.x--; min.x++;}; 00276 if ((max.y - min.y) > spreed.y) {max.y--; min.y++;}; 00277 if ((max.z - min.z) > spreed.z) {max.z--; min.z++;}; 00278 } 00279 00280 void LSM303D::frequency(int hz) 00281 { 00282 _compass.frequency(hz); 00283 }
Generated on Tue Jul 12 2022 17:15:09 by 1.7.2