paul cox / LSM303DLH

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

Fork of LSM303DLH by Michael Shimniok

LSM303DLH.cpp@3:9c3f240d14bf, 2014-10-15 (annotated)

Committer:
paulcox
Date:
Wed Oct 15 15:01:31 2014 +0000
Revision:
3:9c3f240d14bf
Parent:
2:aea5caec809c 
Program tested with all Nucleo boards updated to 2j23m6 firmware and confirmed as working (led blinks and magnetometer info received via I2C)

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
paulcox 3:9c3f240d14bf 44 const int addr_acc = 0x32;
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 }