Michael Shimniok / LSM303DLH

Interface library for STMicro LSM303DLH 3-axis magnetometer w/ 3-axis acceleromter. Computes magnetic heading.

Dependents:   AVC_20110423

LSM303DLH.cpp@1:48d83c63d1d9, 2011-04-08 (annotated)

Committer:
shimniok
Date:
Fri Apr 08 07:29:51 2011 +0000
Revision:
1:48d83c63d1d9
Parent:
0:de767f4959ef
Child:
2:aea5caec809c
Added accelerometer filtering in an attempt to reduce vibration effects on heading.

Who changed what in which revision?

UserRevisionLine numberNew 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
shimniok 0:de767f4959ef 44 const int addr_acc = 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,
shimniok 0:de767f4959ef 58 OUT_X_A = 0x28,
shimniok 0:de767f4959ef 59 OUT_Y_A = 0x2A,
shimniok 0:de767f4959ef 60 OUT_Z_A = 0x2C,
shimniok 0:de767f4959ef 61 };
shimniok 0:de767f4959ef 62
shimniok 0:de767f4959ef 63 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
shimniok 0:de767f4959ef 64 {
shimniok 0:de767f4959ef 65 char data[2] = {addr_reg, v};
shimniok 0:de767f4959ef 66 return LSM303DLH::_compass.write(addr_i2c, data, 2) == 0;
shimniok 0:de767f4959ef 67 }
shimniok 0:de767f4959ef 68
shimniok 0:de767f4959ef 69 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
shimniok 0:de767f4959ef 70 {
shimniok 0:de767f4959ef 71 char data = addr_reg;
shimniok 0:de767f4959ef 72 if ((LSM303DLH::_compass.write(addr_i2c, &data, 1) == 0) && (LSM303DLH::_compass.read(addr_i2c, &data, 1) == 0)){
shimniok 0:de767f4959ef 73 *v = data;
shimniok 0:de767f4959ef 74 return true;
shimniok 0:de767f4959ef 75 }
shimniok 0:de767f4959ef 76 return false;
shimniok 0:de767f4959ef 77 }
shimniok 0:de767f4959ef 78
shimniok 0:de767f4959ef 79 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
shimniok 0:de767f4959ef 80 {
shimniok 0:de767f4959ef 81 char *pv = (char *)v;
shimniok 0:de767f4959ef 82 read_reg(addr_i2c,addr_reg+0,pv+1);
shimniok 0:de767f4959ef 83 return read_reg(addr_i2c,addr_reg+1,pv+0);
shimniok 0:de767f4959ef 84 }
shimniok 0:de767f4959ef 85
shimniok 0:de767f4959ef 86 LSM303DLH::LSM303DLH(PinName sda, PinName scl):
shimniok 1:48d83c63d1d9 87 _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(0), _scale_y(0), _scale_z(0), _filt_ax(0), _filt_ay(0), _filt_az(6000)
shimniok 0:de767f4959ef 88 {
shimniok 0:de767f4959ef 89 char reg_v;
shimniok 0:de767f4959ef 90 _compass.frequency(100000);
shimniok 0:de767f4959ef 91
shimniok 0:de767f4959ef 92 reg_v = 0;
shimniok 0:de767f4959ef 93 reg_v |= 0x01 << 5; /* Normal mode */
shimniok 0:de767f4959ef 94 reg_v |= 0x07; /* X/Y/Z axis enable. */
shimniok 0:de767f4959ef 95 write_reg(addr_acc,CTRL_REG1_A,reg_v);
shimniok 0:de767f4959ef 96 reg_v = 0;
shimniok 0:de767f4959ef 97 read_reg(addr_acc,CTRL_REG1_A,&reg_v);
shimniok 0:de767f4959ef 98
shimniok 0:de767f4959ef 99 reg_v = 0;
shimniok 0:de767f4959ef 100 reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */
shimniok 0:de767f4959ef 101 reg_v |= 0x01 << 4; /* +/- 4g */
shimniok 0:de767f4959ef 102 write_reg(addr_acc,CTRL_REG4_A,reg_v);
shimniok 0:de767f4959ef 103
shimniok 0:de767f4959ef 104 /* -- mag --- */
shimniok 0:de767f4959ef 105 reg_v = 0;
shimniok 0:de767f4959ef 106 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
shimniok 0:de767f4959ef 107 write_reg(addr_mag,CRA_REG_M,reg_v);
shimniok 0:de767f4959ef 108
shimniok 0:de767f4959ef 109 reg_v = 0;
shimniok 0:de767f4959ef 110 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
shimniok 0:de767f4959ef 111 reg_v |= 0x07 << 5; /* +-8.1Gauss */
shimniok 0:de767f4959ef 112 write_reg(addr_mag,CRB_REG_M,reg_v);
shimniok 0:de767f4959ef 113
shimniok 0:de767f4959ef 114 reg_v = 0; /* Continuous-conversion mode */
shimniok 0:de767f4959ef 115 write_reg(addr_mag,MR_REG_M,reg_v);
shimniok 0:de767f4959ef 116 }
shimniok 0:de767f4959ef 117
shimniok 0:de767f4959ef 118
shimniok 0:de767f4959ef 119 void LSM303DLH::setOffset(float x, float y, float z)
shimniok 0:de767f4959ef 120 {
shimniok 0:de767f4959ef 121 _offset_x = x;
shimniok 0:de767f4959ef 122 _offset_y = y;
shimniok 0:de767f4959ef 123 _offset_z = z;
shimniok 0:de767f4959ef 124 }
shimniok 0:de767f4959ef 125
shimniok 0:de767f4959ef 126 void LSM303DLH::setScale(float x, float y, float z)
shimniok 0:de767f4959ef 127 {
shimniok 0:de767f4959ef 128 _scale_x = x;
shimniok 0:de767f4959ef 129 _scale_y = y;
shimniok 0:de767f4959ef 130 _scale_z = z;
shimniok 0:de767f4959ef 131 }
shimniok 0:de767f4959ef 132
shimniok 0:de767f4959ef 133 void LSM303DLH::read(vector &a, vector &m)
shimniok 0:de767f4959ef 134 {
shimniok 0:de767f4959ef 135 short a_x, a_y, a_z;
shimniok 0:de767f4959ef 136 short m_x, m_y, m_z;
shimniok 0:de767f4959ef 137
shimniok 0:de767f4959ef 138 read_reg_short(addr_acc, OUT_X_A, &a_x);
shimniok 0:de767f4959ef 139 read_reg_short(addr_acc, OUT_Y_A, &a_y);
shimniok 0:de767f4959ef 140 read_reg_short(addr_acc, OUT_Z_A, &a_z);
shimniok 0:de767f4959ef 141 read_reg_short(addr_mag, OUT_X_M, &m_x);
shimniok 0:de767f4959ef 142 read_reg_short(addr_mag, OUT_Y_M, &m_y);
shimniok 0:de767f4959ef 143 read_reg_short(addr_mag, OUT_Z_M, &m_z);
shimniok 0:de767f4959ef 144
shimniok 1:48d83c63d1d9 145 // Perform simple lowpass filtering
shimniok 1:48d83c63d1d9 146 // Intended to stabilize heading despite
shimniok 1:48d83c63d1d9 147 // device vibration such as on a UGV
shimniok 1:48d83c63d1d9 148 _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
shimniok 1:48d83c63d1d9 149 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
shimniok 1:48d83c63d1d9 150 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
shimniok 1:48d83c63d1d9 151
shimniok 1:48d83c63d1d9 152 a.x = (float) (_filt_ax >> FILTER_SHIFT);
shimniok 1:48d83c63d1d9 153 a.y = (float) (_filt_ay >> FILTER_SHIFT);
shimniok 1:48d83c63d1d9 154 a.z = (float) (_filt_az >> FILTER_SHIFT);
shimniok 0:de767f4959ef 155
shimniok 0:de767f4959ef 156 // offset and scale
shimniok 0:de767f4959ef 157 m.x = (m_x + _offset_x) * _scale_x;
shimniok 0:de767f4959ef 158 m.y = (m_y + _offset_y) * _scale_y;
shimniok 0:de767f4959ef 159 m.z = (m_z + _offset_z) * _scale_z;
shimniok 0:de767f4959ef 160 }
shimniok 0:de767f4959ef 161
shimniok 0:de767f4959ef 162
shimniok 0:de767f4959ef 163 // Returns the number of degrees from the -Y axis that it
shimniok 0:de767f4959ef 164 // is pointing.
shimniok 0:de767f4959ef 165 float LSM303DLH::heading()
shimniok 0:de767f4959ef 166 {
shimniok 0:de767f4959ef 167 return heading((vector){0,-1,0});
shimniok 0:de767f4959ef 168 }
shimniok 0:de767f4959ef 169
shimniok 0:de767f4959ef 170 float LSM303DLH::heading(vector from)
shimniok 0:de767f4959ef 171 {
shimniok 0:de767f4959ef 172 vector a, m;
shimniok 0:de767f4959ef 173
shimniok 0:de767f4959ef 174 this->read(a, m);
shimniok 0:de767f4959ef 175
shimniok 0:de767f4959ef 176 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 177 // compute heading
shimniok 0:de767f4959ef 178 ////////////////////////////////////////////////
shimniok 0:de767f4959ef 179
shimniok 0:de767f4959ef 180 vector temp_a = a;
shimniok 0:de767f4959ef 181 // normalize
shimniok 0:de767f4959ef 182 vector_normalize(&temp_a);
shimniok 0:de767f4959ef 183 //vector_normalize(&m);
shimniok 0:de767f4959ef 184
shimniok 0:de767f4959ef 185 // compute E and N
shimniok 0:de767f4959ef 186 vector E;
shimniok 0:de767f4959ef 187 vector N;
shimniok 0:de767f4959ef 188 vector_cross(&m,&temp_a,&E);
shimniok 0:de767f4959ef 189 vector_normalize(&E);
shimniok 0:de767f4959ef 190 vector_cross(&temp_a,&E,&N);
shimniok 0:de767f4959ef 191
shimniok 0:de767f4959ef 192 // compute heading
shimniok 0:de767f4959ef 193 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
shimniok 0:de767f4959ef 194 if (heading < 0) heading += 360;
shimniok 0:de767f4959ef 195
shimniok 0:de767f4959ef 196 return heading;
shimniok 0:de767f4959ef 197 }