Michael Shimniok / LSM303DLH

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

Dependents:   AVC_20110423

LSM303DLH.cpp@0:de767f4959ef, 2011-04-06 (annotated)

Committer:
shimniok
Date:
Wed Apr 06 05:05:10 2011 +0000
Revision:
0:de767f4959ef
Child:
1:48d83c63d1d9
Initial revision

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