Curtis Belknap / Mbed 2 deprecated LSM303DLHTest

LSM303DLH Test Program for get angle.

Dependencies:   mbed

Fork of LSM303DLHTest by Toshihisa T

LSM303DLH/LSM303DLH.cpp@5:05d5e64e76f2, 2015-03-18 (annotated)

Committer:
cbelknap
Date:
Wed Mar 18 16:48:45 2015 +0000
Revision:
5:05d5e64e76f2
Parent:
3:f3796683b4c9 
Get magnitude, acceleration and heading

Who changed what in which revision?

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