Jonathan Goret / LSM303DLH

same

Dependents:   Nucleo_gyro_magno

LSM303DLH.cpp@0:38cd86f4249d, 2015-04-13 (annotated)

Committer:
johnylafleur
Date:
Mon Apr 13 16:05:08 2015 +0000
Revision:
0:38cd86f4249d
same

Who changed what in which revision?

UserRevisionLine numberNew contents of line
johnylafleur 0:38cd86f4249d 1 /** LSM303DLH Interface Library
johnylafleur 0:38cd86f4249d 2 *
johnylafleur 0:38cd86f4249d 3 * Michael Shimniok http://bot-thoughts.com
johnylafleur 0:38cd86f4249d 4 *
johnylafleur 0:38cd86f4249d 5 * Based on test program by @tosihisa and
johnylafleur 0:38cd86f4249d 6 *
johnylafleur 0:38cd86f4249d 7 * Pololu sample library for LSM303DLH breakout by ryantm:
johnylafleur 0:38cd86f4249d 8 *
johnylafleur 0:38cd86f4249d 9 * Copyright (c) 2011 Pololu Corporation. For more information, see
johnylafleur 0:38cd86f4249d 10 *
johnylafleur 0:38cd86f4249d 11 * http://www.pololu.com/
johnylafleur 0:38cd86f4249d 12 * http://forum.pololu.com/
johnylafleur 0:38cd86f4249d 13 *
johnylafleur 0:38cd86f4249d 14 * Permission is hereby granted, free of charge, to any person
johnylafleur 0:38cd86f4249d 15 * obtaining a copy of this software and associated documentation
johnylafleur 0:38cd86f4249d 16 * files (the "Software"), to deal in the Software without
johnylafleur 0:38cd86f4249d 17 * restriction, including without limitation the rights to use,
johnylafleur 0:38cd86f4249d 18 * copy, modify, merge, publish, distribute, sublicense, and/or sell
johnylafleur 0:38cd86f4249d 19 * copies of the Software, and to permit persons to whom the
johnylafleur 0:38cd86f4249d 20 * Software is furnished to do so, subject to the following
johnylafleur 0:38cd86f4249d 21 * conditions:
johnylafleur 0:38cd86f4249d 22 *
johnylafleur 0:38cd86f4249d 23 * The above copyright notice and this permission notice shall be
johnylafleur 0:38cd86f4249d 24 * included in all copies or substantial portions of the Software.
johnylafleur 0:38cd86f4249d 25 *
johnylafleur 0:38cd86f4249d 26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
johnylafleur 0:38cd86f4249d 27 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
johnylafleur 0:38cd86f4249d 28 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
johnylafleur 0:38cd86f4249d 29 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
johnylafleur 0:38cd86f4249d 30 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
johnylafleur 0:38cd86f4249d 31 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
johnylafleur 0:38cd86f4249d 32 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
johnylafleur 0:38cd86f4249d 33 * OTHER DEALINGS IN THE SOFTWARE.
johnylafleur 0:38cd86f4249d 34 */
johnylafleur 0:38cd86f4249d 35 #include "mbed.h"
johnylafleur 0:38cd86f4249d 36 #include "LSM303DLH.h"
johnylafleur 0:38cd86f4249d 37
johnylafleur 0:38cd86f4249d 38 #ifndef M_PI
johnylafleur 0:38cd86f4249d 39 #define M_PI 3.14159265358979323846
johnylafleur 0:38cd86f4249d 40 #endif
johnylafleur 0:38cd86f4249d 41
johnylafleur 0:38cd86f4249d 42 #define FILTER_SHIFT 6 // used in filtering acceleromter readings
johnylafleur 0:38cd86f4249d 43
johnylafleur 0:38cd86f4249d 44 const int addr_acc = 0x30;
johnylafleur 0:38cd86f4249d 45 const int addr_mag = 0x3c;
johnylafleur 0:38cd86f4249d 46
johnylafleur 0:38cd86f4249d 47 enum REG_ADDRS {
johnylafleur 0:38cd86f4249d 48 /* --- Mag --- */
johnylafleur 0:38cd86f4249d 49 CRA_REG_M = 0x00,
johnylafleur 0:38cd86f4249d 50 CRB_REG_M = 0x01,
johnylafleur 0:38cd86f4249d 51 MR_REG_M = 0x02,
johnylafleur 0:38cd86f4249d 52 OUT_X_M = 0x03,
johnylafleur 0:38cd86f4249d 53 OUT_Y_M = 0x05,
johnylafleur 0:38cd86f4249d 54 OUT_Z_M = 0x07,
johnylafleur 0:38cd86f4249d 55 /* --- Acc --- */
johnylafleur 0:38cd86f4249d 56 CTRL_REG1_A = 0x20,
johnylafleur 0:38cd86f4249d 57 CTRL_REG4_A = 0x23,
johnylafleur 0:38cd86f4249d 58 OUT_X_A = 0x28,
johnylafleur 0:38cd86f4249d 59 OUT_Y_A = 0x2A,
johnylafleur 0:38cd86f4249d 60 OUT_Z_A = 0x2C,
johnylafleur 0:38cd86f4249d 61 };
johnylafleur 0:38cd86f4249d 62
johnylafleur 0:38cd86f4249d 63 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
johnylafleur 0:38cd86f4249d 64 {
johnylafleur 0:38cd86f4249d 65 char data[2] = {addr_reg, v};
johnylafleur 0:38cd86f4249d 66 return LSM303DLH::_compass.write(addr_i2c, data, 2) == 0;
johnylafleur 0:38cd86f4249d 67 }
johnylafleur 0:38cd86f4249d 68
johnylafleur 0:38cd86f4249d 69 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
johnylafleur 0:38cd86f4249d 70 {
johnylafleur 0:38cd86f4249d 71 char data = addr_reg;
johnylafleur 0:38cd86f4249d 72 bool result = false;
johnylafleur 0:38cd86f4249d 73
johnylafleur 0:38cd86f4249d 74 __disable_irq();
johnylafleur 0:38cd86f4249d 75 if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
johnylafleur 0:38cd86f4249d 76 *v = data;
johnylafleur 0:38cd86f4249d 77 result = true;
johnylafleur 0:38cd86f4249d 78 }
johnylafleur 0:38cd86f4249d 79 __enable_irq();
johnylafleur 0:38cd86f4249d 80 return result;
johnylafleur 0:38cd86f4249d 81 }
johnylafleur 0:38cd86f4249d 82
johnylafleur 0:38cd86f4249d 83 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
johnylafleur 0:38cd86f4249d 84 {
johnylafleur 0:38cd86f4249d 85 char *pv = (char *)v;
johnylafleur 0:38cd86f4249d 86 bool result;
johnylafleur 0:38cd86f4249d 87
johnylafleur 0:38cd86f4249d 88 result = read_reg(addr_i2c,addr_reg+0,pv+1);
johnylafleur 0:38cd86f4249d 89 result &= read_reg(addr_i2c,addr_reg+1,pv+0);
johnylafleur 0:38cd86f4249d 90
johnylafleur 0:38cd86f4249d 91 return result;
johnylafleur 0:38cd86f4249d 92 }
johnylafleur 0:38cd86f4249d 93
johnylafleur 0:38cd86f4249d 94 LSM303DLH::LSM303DLH(PinName sda, PinName scl):
johnylafleur 0:38cd86f4249d 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)
johnylafleur 0:38cd86f4249d 96 {
johnylafleur 0:38cd86f4249d 97 char reg_v;
johnylafleur 0:38cd86f4249d 98 _compass.frequency(100000);
johnylafleur 0:38cd86f4249d 99
johnylafleur 0:38cd86f4249d 100 reg_v = 0;
johnylafleur 0:38cd86f4249d 101 reg_v |= 0x01 << 5; /* Normal mode */
johnylafleur 0:38cd86f4249d 102 reg_v |= 0x07; /* X/Y/Z axis enable. */
johnylafleur 0:38cd86f4249d 103 write_reg(addr_acc,CTRL_REG1_A,reg_v);
johnylafleur 0:38cd86f4249d 104 reg_v = 0;
johnylafleur 0:38cd86f4249d 105 read_reg(addr_acc,CTRL_REG1_A,&reg_v);
johnylafleur 0:38cd86f4249d 106
johnylafleur 0:38cd86f4249d 107 reg_v = 0;
johnylafleur 0:38cd86f4249d 108 reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */
johnylafleur 0:38cd86f4249d 109 reg_v |= 0x01 << 4; /* +/- 4g */
johnylafleur 0:38cd86f4249d 110 write_reg(addr_acc,CTRL_REG4_A,reg_v);
johnylafleur 0:38cd86f4249d 111
johnylafleur 0:38cd86f4249d 112 /* -- mag --- */
johnylafleur 0:38cd86f4249d 113 reg_v = 0;
johnylafleur 0:38cd86f4249d 114 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
johnylafleur 0:38cd86f4249d 115 write_reg(addr_mag,CRA_REG_M,reg_v);
johnylafleur 0:38cd86f4249d 116
johnylafleur 0:38cd86f4249d 117 reg_v = 0;
johnylafleur 0:38cd86f4249d 118 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
johnylafleur 0:38cd86f4249d 119 reg_v |= 0x07 << 5; /* +-8.1Gauss */
johnylafleur 0:38cd86f4249d 120 write_reg(addr_mag,CRB_REG_M,reg_v);
johnylafleur 0:38cd86f4249d 121
johnylafleur 0:38cd86f4249d 122 reg_v = 0; /* Continuous-conversion mode */
johnylafleur 0:38cd86f4249d 123 write_reg(addr_mag,MR_REG_M,reg_v);
johnylafleur 0:38cd86f4249d 124 }
johnylafleur 0:38cd86f4249d 125
johnylafleur 0:38cd86f4249d 126
johnylafleur 0:38cd86f4249d 127 void LSM303DLH::setOffset(float x, float y, float z)
johnylafleur 0:38cd86f4249d 128 {
johnylafleur 0:38cd86f4249d 129 _offset_x = x;
johnylafleur 0:38cd86f4249d 130 _offset_y = y;
johnylafleur 0:38cd86f4249d 131 _offset_z = z;
johnylafleur 0:38cd86f4249d 132 }
johnylafleur 0:38cd86f4249d 133
johnylafleur 0:38cd86f4249d 134 void LSM303DLH::setScale(float x, float y, float z)
johnylafleur 0:38cd86f4249d 135 {
johnylafleur 0:38cd86f4249d 136 _scale_x = x;
johnylafleur 0:38cd86f4249d 137 _scale_y = y;
johnylafleur 0:38cd86f4249d 138 _scale_z = z;
johnylafleur 0:38cd86f4249d 139 }
johnylafleur 0:38cd86f4249d 140
johnylafleur 0:38cd86f4249d 141 void LSM303DLH::read(vector &a, vector &m)
johnylafleur 0:38cd86f4249d 142 {
johnylafleur 0:38cd86f4249d 143 short a_x, a_y, a_z;
johnylafleur 0:38cd86f4249d 144 short m_x, m_y, m_z;
johnylafleur 0:38cd86f4249d 145 //Timer t;
johnylafleur 0:38cd86f4249d 146 //int usec1, usec2;
johnylafleur 0:38cd86f4249d 147
johnylafleur 0:38cd86f4249d 148 //t.reset();
johnylafleur 0:38cd86f4249d 149 //t.start();
johnylafleur 0:38cd86f4249d 150
johnylafleur 0:38cd86f4249d 151 //usec1 = t.read_us();
johnylafleur 0:38cd86f4249d 152 read_reg_short(addr_acc, OUT_X_A, &a_x);
johnylafleur 0:38cd86f4249d 153 read_reg_short(addr_acc, OUT_Y_A, &a_y);
johnylafleur 0:38cd86f4249d 154 read_reg_short(addr_acc, OUT_Z_A, &a_z);
johnylafleur 0:38cd86f4249d 155 read_reg_short(addr_mag, OUT_X_M, &m_x);
johnylafleur 0:38cd86f4249d 156 read_reg_short(addr_mag, OUT_Y_M, &m_y);
johnylafleur 0:38cd86f4249d 157 read_reg_short(addr_mag, OUT_Z_M, &m_z);
johnylafleur 0:38cd86f4249d 158 //usec2 = t.read_us();
johnylafleur 0:38cd86f4249d 159
johnylafleur 0:38cd86f4249d 160 //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
johnylafleur 0:38cd86f4249d 161
johnylafleur 0:38cd86f4249d 162 // Perform simple lowpass filtering
johnylafleur 0:38cd86f4249d 163 // Intended to stabilize heading despite
johnylafleur 0:38cd86f4249d 164 // device vibration such as on a UGV
johnylafleur 0:38cd86f4249d 165 _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 166 _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 167 _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 168
johnylafleur 0:38cd86f4249d 169 a.x = (float) (_filt_ax >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 170 a.y = (float) (_filt_ay >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 171 a.z = (float) (_filt_az >> FILTER_SHIFT);
johnylafleur 0:38cd86f4249d 172
johnylafleur 0:38cd86f4249d 173 // offset and scale
johnylafleur 0:38cd86f4249d 174 m.x = (m_x + _offset_x) * _scale_x;
johnylafleur 0:38cd86f4249d 175 m.y = (m_y + _offset_y) * _scale_y;
johnylafleur 0:38cd86f4249d 176 m.z = (m_z + _offset_z) * _scale_z;
johnylafleur 0:38cd86f4249d 177 }
johnylafleur 0:38cd86f4249d 178
johnylafleur 0:38cd86f4249d 179
johnylafleur 0:38cd86f4249d 180 // Returns the number of degrees from the -Y axis that it
johnylafleur 0:38cd86f4249d 181 // is pointing.
johnylafleur 0:38cd86f4249d 182 float LSM303DLH::heading()
johnylafleur 0:38cd86f4249d 183 {
johnylafleur 0:38cd86f4249d 184 return heading((vector){0,-1,0});
johnylafleur 0:38cd86f4249d 185 }
johnylafleur 0:38cd86f4249d 186
johnylafleur 0:38cd86f4249d 187 float LSM303DLH::heading(vector from)
johnylafleur 0:38cd86f4249d 188 {
johnylafleur 0:38cd86f4249d 189 vector a, m;
johnylafleur 0:38cd86f4249d 190
johnylafleur 0:38cd86f4249d 191 this->read(a, m);
johnylafleur 0:38cd86f4249d 192
johnylafleur 0:38cd86f4249d 193 ////////////////////////////////////////////////
johnylafleur 0:38cd86f4249d 194 // compute heading
johnylafleur 0:38cd86f4249d 195 ////////////////////////////////////////////////
johnylafleur 0:38cd86f4249d 196
johnylafleur 0:38cd86f4249d 197 vector temp_a = a;
johnylafleur 0:38cd86f4249d 198 // normalize
johnylafleur 0:38cd86f4249d 199 vector_normalize(&temp_a);
johnylafleur 0:38cd86f4249d 200 //vector_normalize(&m);
johnylafleur 0:38cd86f4249d 201
johnylafleur 0:38cd86f4249d 202 // compute E and N
johnylafleur 0:38cd86f4249d 203 vector E;
johnylafleur 0:38cd86f4249d 204 vector N;
johnylafleur 0:38cd86f4249d 205 vector_cross(&m,&temp_a,&E);
johnylafleur 0:38cd86f4249d 206 vector_normalize(&E);
johnylafleur 0:38cd86f4249d 207 vector_cross(&temp_a,&E,&N);
johnylafleur 0:38cd86f4249d 208
johnylafleur 0:38cd86f4249d 209 // compute heading
johnylafleur 0:38cd86f4249d 210 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
johnylafleur 0:38cd86f4249d 211 if (heading < 0) heading += 360;
johnylafleur 0:38cd86f4249d 212
johnylafleur 0:38cd86f4249d 213 return heading;
johnylafleur 0:38cd86f4249d 214 }
johnylafleur 0:38cd86f4249d 215
johnylafleur 0:38cd86f4249d 216 void LSM303DLH::frequency(int hz)
johnylafleur 0:38cd86f4249d 217 {
johnylafleur 0:38cd86f4249d 218 _compass.frequency(hz);
johnylafleur 0:38cd86f4249d 219 }