Michael Shimniok / LSM303DLH

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

Dependents:   AVC_20110423

LSM303DLH.cpp@2:aea5caec809c, 2011-04-12 (annotated)

Committer:
shimniok
Date:
Tue Apr 12 18:21:44 2011 +0000
Revision:
2:aea5caec809c
Parent:
1:48d83c63d1d9 
Added option to set I2C frequency, disabled IRQ during write/read of mag/acc registers due to apparent conflicts with interrupts during testing

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