1 /* 2 * Author: Brendan Le Foll <brendan.le.foll (at) 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 * Permission is hereby granted, free of charge, to any person obtaining 9 * a copy of this software and associated documentation files (the 10 * "Software"), to deal in the Software without restriction, including 11 * without limitation the rights to use, copy, modify, merge, publish, 12 * distribute, sublicense, and/or sell copies of the Software, and to 13 * permit persons to whom the Software is furnished to do so, subject to 14 * the following conditions: 15 * 16 * The above copyright notice and this permission notice shall be 17 * included in all copies or substantial portions of the Software. 18 * 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 20 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 21 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 22 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 23 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 24 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 25 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 26 */ 27 28 #include <iostream> 29 #include <string> 30 #include <stdexcept> 31 #include <unistd.h> 32 #include <stdlib.h> 33 34 #include "lsm303.h" 35 36 using namespace upm; 37 38 LSM303::LSM303(int bus, int addrMag, int addrAcc, int accScale) : m_i2c(bus) 39 { 40 m_addrMag = addrMag; 41 m_addrAcc = addrAcc; 42 43 // 0x27 is the 'normal' mode with X/Y/Z enable 44 setRegisterSafe(m_addrAcc, CTRL_REG1_A, 0x27); 45 46 // scale can be 2, 4 or 8 47 if (2 == accScale) { 48 setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x00); 49 } else if (4 == accScale) { 50 setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x10); 51 } else { // default; equivalent to 8g 52 setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x30); 53 } 54 55 // 0x10 = minimum datarate ~15Hz output rate 56 setRegisterSafe(m_addrMag, CRA_REG_M, 0x10); 57 58 // magnetic scale = +/-1.3 59 // Gaussmagnetic scale = +/-1.3Gauss (0x20) 60 // +-8.1Gauss (0xe0) 61 setRegisterSafe(m_addrMag, CRB_REG_M, 0xe0); 62 63 // 0x00 = continouous conversion mode 64 setRegisterSafe(m_addrMag, MR_REG_M, 0x00); 65 } 66 67 float 68 LSM303::getHeading() 69 { 70 if (getCoordinates() != mraa::SUCCESS) { 71 return -1; 72 } 73 74 float heading = 180.0 * atan2(double(coor[Y]), double(coor[X]))/M_PI; 75 76 if (heading < 0.0) 77 heading += 360.0; 78 79 return heading; 80 } 81 82 int16_t* 83 LSM303::getRawAccelData() 84 { 85 return &accel[0]; 86 } 87 88 int16_t* 89 LSM303::getRawCoorData() 90 { 91 return &coor[0]; 92 } 93 94 int16_t 95 LSM303::getAccelX() 96 { 97 return accel[X]; 98 } 99 100 int16_t 101 LSM303::getAccelY() 102 { 103 return accel[Y]; 104 } 105 106 int16_t 107 LSM303::getAccelZ() 108 { 109 return accel[Z]; 110 } 111 112 mraa::Result 113 LSM303::getCoordinates() 114 { 115 mraa::Result ret = mraa::SUCCESS; 116 117 memset(&buf[0], 0, sizeof(uint8_t)*6); 118 ret = m_i2c.address(m_addrMag); 119 ret = m_i2c.writeByte(OUT_X_H_M); 120 ret = m_i2c.address(m_addrMag); 121 int num = m_i2c.read(buf, 6); 122 if (num != 6) { 123 return ret; 124 } 125 // convert to coordinates 126 for (int i=0; i<3; i++) { 127 coor[i] = (int16_t(buf[2*i] << 8)) 128 | int16_t(buf[(2*i)+1]); 129 } 130 // swap elements 1 and 2 to get things in natural XYZ order 131 int16_t t = coor[2]; 132 coor[2] = coor[1]; 133 coor[1] = t; 134 //printf("X=%x, Y=%x, Z=%x\n", coor[X], coor[Y], coor[Z]); 135 136 return ret; 137 } 138 139 int16_t 140 LSM303::getCoorX() { 141 return coor[X]; 142 } 143 144 int16_t 145 LSM303::getCoorY() { 146 return coor[Y]; 147 } 148 149 int16_t 150 LSM303::getCoorZ() { 151 return coor[Z]; 152 } 153 154 // helper function that writes a value to the acc and then reads 155 // FIX: shouldn't this be write-then-read? 156 int 157 LSM303::readThenWrite(uint8_t reg) 158 { 159 m_i2c.address(m_addrAcc); 160 m_i2c.writeByte(reg); 161 m_i2c.address(m_addrAcc); 162 return (int) m_i2c.readByte(); 163 } 164 165 mraa::Result 166 LSM303::getAcceleration() 167 { 168 mraa::Result ret = mraa::SUCCESS; 169 170 accel[X] = (int16_t(readThenWrite(OUT_X_H_A)) << 8) 171 | int16_t(readThenWrite(OUT_X_L_A)); 172 accel[Y] = (int16_t(readThenWrite(OUT_Y_H_A)) << 8) 173 | int16_t(readThenWrite(OUT_Y_L_A)); 174 accel[Z] = (int16_t(readThenWrite(OUT_Z_H_A)) << 8) 175 | int16_t(readThenWrite(OUT_Z_L_A)); 176 //printf("X=%x, Y=%x, Z=%x\n", accel[X], accel[Y], accel[Z]); 177 178 return ret; 179 } 180 181 // helper function that sets a register and then checks the set was succesful 182 mraa::Result 183 LSM303::setRegisterSafe(uint8_t slave, uint8_t sregister, uint8_t data) 184 { 185 buf[0] = sregister; 186 buf[1] = data; 187 188 if (m_i2c.address(slave) != mraa::SUCCESS) { 189 throw std::invalid_argument(std::string(__FUNCTION__) + 190 ": mraa_i2c_address() failed"); 191 return mraa::ERROR_INVALID_HANDLE; 192 } 193 if (m_i2c.write(buf, 2) != mraa::SUCCESS) { 194 throw std::invalid_argument(std::string(__FUNCTION__) + 195 ": mraa_i2c_write() failed"); 196 return mraa::ERROR_INVALID_HANDLE; 197 } 198 uint8_t val = m_i2c.readReg(sregister); 199 if (val != data) { 200 throw std::invalid_argument(std::string(__FUNCTION__) + 201 ": failed to set register correctly"); 202 return mraa::ERROR_UNSPECIFIED; 203 } 204 return mraa::SUCCESS; 205 } 206
