1 /* 2 * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha (at) intel.com> 3 * Copyright (c) 2014 Intel Corporation. 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining 6 * a copy of this software and associated documentation files (the 7 * "Software"), to deal in the Software without restriction, including 8 * without limitation the rights to use, copy, modify, merge, publish, 9 * distribute, sublicense, and/or sell copies of the Software, and to 10 * permit persons to whom the Software is furnished to do so, subject to 11 * the following conditions: 12 * 13 * The above copyright notice and this permission notice shall be 14 * included in all copies or substantial portions of the Software. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 */ 24 25 #include <iostream> 26 #include <string> 27 #include <stdexcept> 28 #include <unistd.h> 29 #include <stdlib.h> 30 #include <string.h> 31 #include <pthread.h> 32 #include <math.h> 33 34 #include "mma7455.h" 35 36 using namespace upm; 37 38 MMA7455::MMA7455 (int bus, int devAddr) : m_i2ControlCtx(bus) { 39 unsigned char data = 0; 40 int nBytes = 0; 41 42 m_name = "MMA7455"; 43 44 m_controlAddr = devAddr; 45 m_bus = bus; 46 47 mraa::Result error = m_i2ControlCtx.address(m_controlAddr); 48 if (error != mraa::SUCCESS) { 49 throw std::runtime_error(std::string(__FUNCTION__) + 50 ": mraa_i2c_address() failed"); 51 return; 52 } 53 54 // setting GLVL 0x1 (64LSB/g) and MODE 0x1 (Measurement Mode) 55 data = (BIT (MMA7455_GLVL0) | BIT (MMA7455_MODE0)); 56 error = i2cWriteReg (MMA7455_MCTL, &data, 0x1); 57 if (error != mraa::SUCCESS) { 58 throw std::runtime_error(std::string(__FUNCTION__) + 59 ": writing mode register failed"); 60 return; 61 } 62 63 if (mraa::SUCCESS != calibrate ()) { 64 throw std::runtime_error(std::string(__FUNCTION__) + 65 ": calibrate() failed"); 66 return; 67 } 68 } 69 70 mraa::Result 71 MMA7455::calibrate () { 72 mraa::Result error = mraa::SUCCESS; 73 int i = 0; 74 75 accelData xyz; 76 xyz.value.x = xyz.value.y = xyz.value.z = 0; 77 78 do { 79 error = readData (&xyz.value.x, &xyz.value.y, &xyz.value.z); 80 if (mraa::SUCCESS != error) { 81 return error; 82 } 83 84 xyz.value.x += 2 * -xyz.value.x; 85 xyz.value.y += 2 * -xyz.value.y; 86 xyz.value.z += 2 * -(xyz.value.z - 64); 87 88 error = i2cWriteReg (MMA7455_XOFFL, (unsigned char *) &xyz, 0x6); 89 if (error != mraa::SUCCESS) { 90 return error; 91 } 92 93 } while ( ++i < 3 ); 94 95 return error; 96 } 97 98 mraa::Result 99 MMA7455::readData (short * ptrX, short * ptrY, short * ptrZ) { 100 accelData xyz; 101 unsigned char data = 0; 102 int nBytes = 0; 103 104 /*do { 105 nBytes = i2cReadReg (MMA7455_STATUS, &data, 0x1); 106 } while ( !(data & MMA7455_DRDY) && nBytes == mraa::SUCCESS); 107 108 if (nBytes == mraa::SUCCESS) { 109 std::cout << "NO_GDB :: 1" << std::endl; 110 return mraa::SUCCESS; 111 }*/ 112 113 nBytes = i2cReadReg (MMA7455_XOUTL, (unsigned char *) &xyz, 0x6); 114 if (nBytes == 0) { 115 std::cout << "NO_GDB :: 2" << std::endl; 116 return mraa::ERROR_UNSPECIFIED; 117 } 118 119 if (xyz.reg.x_msb & 0x02) { 120 xyz.reg.x_msb |= 0xFC; 121 } 122 123 if (xyz.reg.y_msb & 0x02) { 124 xyz.reg.y_msb |= 0xFC; 125 } 126 127 if (xyz.reg.z_msb & 0x02) { 128 xyz.reg.z_msb |= 0xFC; 129 } 130 131 // The result is the g-force in units of 64 per 'g'. 132 *ptrX = xyz.value.x; 133 *ptrY = xyz.value.y; 134 *ptrZ = xyz.value.z; 135 136 return mraa::SUCCESS; 137 } 138 139 #ifdef SWIGJAVA 140 short *MMA7455::readData() { 141 short *v = new short[3]; 142 readData(&v[0], &v[1], &v[2]); 143 return v; 144 } 145 #endif 146 147 int 148 MMA7455::i2cReadReg (unsigned char reg, uint8_t *buffer, int len) { 149 if (mraa::SUCCESS != m_i2ControlCtx.address(m_controlAddr)) { 150 throw std::runtime_error(std::string(__FUNCTION__) + 151 ": mraa_i2c_address() failed"); 152 return 0; 153 } 154 155 if (mraa::SUCCESS != m_i2ControlCtx.writeByte(reg)) { 156 throw std::runtime_error(std::string(__FUNCTION__) + 157 ": mraa_i2c_write_byte() failed"); 158 return 0; 159 } 160 161 return (int) m_i2ControlCtx.read(buffer, len); 162 } 163 164 mraa::Result 165 MMA7455::i2cWriteReg (unsigned char reg, uint8_t *buffer, int len) { 166 mraa::Result error = mraa::SUCCESS; 167 168 uint8_t data[len + 1]; 169 data[0] = reg; 170 memcpy(&data[1], buffer, len); 171 172 error = m_i2ControlCtx.address (m_controlAddr); 173 if (error != mraa::SUCCESS) { 174 throw std::runtime_error(std::string(__FUNCTION__) + 175 ": mraa_i2c_address() failed"); 176 return error; 177 } 178 error = m_i2ControlCtx.write (data, len + 1); 179 if (error != mraa::SUCCESS) { 180 throw std::runtime_error(std::string(__FUNCTION__) + 181 ": mraa_i2c_write() failed"); 182 return error; 183 } 184 185 return error; 186 } 187
