1 /*
2 * Author: Brendan Le Foll <brendan.le.foll@intel.com>
3 * Copyright (c) 2014 Intel Corporation.
4 *
5 * Code based on LSM303DLH sample by Jim Lindblom SparkFun Electronics
6 * and the CompensatedCompass.ino by Frankie Chu from SeedStudio
7 *
8 * Further modifications to port to the LSM303d by <bruce.j.beare@intel.com>
9 *
10 * Permission is hereby granted, free of charge, to any person obtaining
11 * a copy of this software and associated documentation files (the
12 * "Software"), to deal in the Software without restriction, including
13 * without limitation the rights to use, copy, modify, merge, publish,
14 * distribute, sublicense, and/or sell copies of the Software, and to
15 * permit persons to whom the Software is furnished to do so, subject to
16 * the following conditions:
17 *
18 * The above copyright notice and this permission notice shall be
19 * included in all copies or substantial portions of the Software.
20 *
21 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
22 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
23 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
24 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
25 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
26 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
27 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
28 */
29
30 #include <iostream>
31 #include <string>
32 #include <stdexcept>
33 #include <unistd.h>
34 #include <stdlib.h>
35
36 #include "lsm303d.h"
37
38 using namespace upm;
39
LSM303d(int bus,int addr,int accScale)40 LSM303d::LSM303d(int bus, int addr, int accScale) : m_i2c(bus)
41 {
42 m_addr = addr;
43
44 uint8_t afs_bits = 0; // LM303D_SCALE_2G - see the data sheet
45 const uint8_t abw_bits = 3; // 50hz Anti-alias filter bandwidth - see the data sheet
46
47 // 0x27 is the 'normal' mode with X/Y/Z enable
48 // Data is available at 100HZ.
49 // See the data sheet for higher data rates.
50 setRegisterSafe(m_addr, CTRL_REG1, 0x67);
51
52 // scale can be 2, 4 or 8
53 switch (accScale) {
54 case LM303D_SCALE_2G:
55 afs_bits = 0;
56 break;
57 case LM303D_SCALE_4G:
58 afs_bits = 1;
59 break;
60 case LM303D_SCALE_6G:
61 afs_bits = 2;
62 break;
63 case LM303D_SCALE_8G:
64 afs_bits = 3;
65 break;
66 case LM303D_SCALE_16G:
67 afs_bits = 4;
68 break;
69 default:
70 throw std::invalid_argument(std::string(__FUNCTION__) +
71 ": failed to specify scaling correctly");
72 break;
73 }
74 setRegisterSafe(m_addr, CTRL_REG2, (abw_bits<<6)|(afs_bits<<3));
75
76 // Enable Mag.
77 const uint8_t mag_resolution_bits = 3 << 5; // high resolution
78 const uint8_t mag_data_rate_bits = 4 << 2; // 50 hz
79 const uint8_t mag_sensor_mode = 0; // continuous conversion
80
81 setRegisterSafe(m_addr, CTRL_REG5, mag_resolution_bits|mag_data_rate_bits);
82 setRegisterSafe(m_addr, CTRL_REG7, mag_sensor_mode);
83 }
84
85 float
getHeading()86 LSM303d::getHeading()
87 {
88 if (getCoordinates() != mraa::SUCCESS) {
89 return -1.0;
90 }
91
92 float heading = 180.0 * atan2(double(coor[Y]), double(coor[X]))/M_PI;
93
94 if (heading < 0.0)
95 heading += 360.0;
96
97 return heading;
98 }
99
100 int16_t*
getRawAccelData()101 LSM303d::getRawAccelData()
102 {
103 return &accel[0];
104 }
105
106 int16_t*
getRawCoorData()107 LSM303d::getRawCoorData()
108 {
109 return &coor[0];
110 }
111
112 int16_t
getAccelX()113 LSM303d::getAccelX()
114 {
115 return accel[X];
116 }
117
118 int16_t
getAccelY()119 LSM303d::getAccelY()
120 {
121 return accel[Y];
122 }
123
124 int16_t
getAccelZ()125 LSM303d::getAccelZ()
126 {
127 return accel[Z];
128 }
129
130 mraa::Result
getCoordinates()131 LSM303d::getCoordinates()
132 {
133 mraa::Result ret = mraa::SUCCESS;
134 uint8_t status = writeThenRead(STATUS_M);
135
136 coor[X] = (int16_t(writeThenRead(OUT_X_H_M)) << 8)
137 | int16_t(writeThenRead(OUT_X_L_M));
138 coor[Y] = (int16_t(writeThenRead(OUT_Y_H_M)) << 8)
139 | int16_t(writeThenRead(OUT_Y_L_M));
140 coor[Z] = (int16_t(writeThenRead(OUT_Z_H_M)) << 8)
141 | int16_t(writeThenRead(OUT_Z_L_M));
142 //printf("status=0x%x, X=%d, Y=%d, Z=%d\n", status, coor[X], coor[Y], coor[Z]);
143
144 return ret;
145 }
146
147
148 int16_t
getCoorX()149 LSM303d::getCoorX() {
150 return coor[X];
151 }
152
153 int16_t
getCoorY()154 LSM303d::getCoorY() {
155 return coor[Y];
156 }
157
158 int16_t
getCoorZ()159 LSM303d::getCoorZ() {
160 return coor[Z];
161 }
162
163 // helper function that writes a value to the acc and then reads
164 int
writeThenRead(uint8_t reg)165 LSM303d::writeThenRead(uint8_t reg)
166 {
167 m_i2c.address(m_addr);
168 m_i2c.writeByte(reg);
169 m_i2c.address(m_addr);
170 return (int) m_i2c.readByte();
171 }
172
173 mraa::Result
getAcceleration()174 LSM303d::getAcceleration()
175 {
176 mraa::Result ret = mraa::SUCCESS;
177
178 accel[X] = (int16_t(writeThenRead(OUT_X_H_A)) << 8)
179 | int16_t(writeThenRead(OUT_X_L_A));
180 accel[Y] = (int16_t(writeThenRead(OUT_Y_H_A)) << 8)
181 | int16_t(writeThenRead(OUT_Y_L_A));
182 accel[Z] = (int16_t(writeThenRead(OUT_Z_H_A)) << 8)
183 | int16_t(writeThenRead(OUT_Z_L_A));
184 //printf("X=%x, Y=%x, Z=%x\n", accel[X], accel[Y], accel[Z]);
185
186 return ret;
187 }
188
189 // helper function that sets a register and then checks the set was succesful
190 mraa::Result
setRegisterSafe(uint8_t slave,uint8_t sregister,uint8_t data)191 LSM303d::setRegisterSafe(uint8_t slave, uint8_t sregister, uint8_t data)
192 {
193 buf[0] = sregister;
194 buf[1] = data;
195
196 if (m_i2c.address(slave) != mraa::SUCCESS) {
197 throw std::invalid_argument(std::string(__FUNCTION__) +
198 ": mraa_i2c_address() failed");
199 return mraa::ERROR_INVALID_HANDLE;
200 }
201 if (m_i2c.write(buf, 2) != mraa::SUCCESS) {
202 throw std::invalid_argument(std::string(__FUNCTION__) +
203 ": mraa_i2c_write() failed");
204 return mraa::ERROR_INVALID_HANDLE;
205 }
206 uint8_t val = m_i2c.readReg(sregister);
207 if (val != data) {
208 throw std::invalid_argument(std::string(__FUNCTION__) +
209 ": failed to set register correctly");
210 return mraa::ERROR_UNSPECIFIED;
211 }
212 return mraa::SUCCESS;
213 }
214