src/mma7455/mma7455.cxx

   1 /*
   2  * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@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 <unistd.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <pthread.h>
  30 #include <math.h>
  31
  32 #include "mma7455.h"
  33
  34 using namespace upm;
  35
  36 MMA7455::MMA7455 (int bus, int devAddr) {
  37     unsigned char data   = 0;
  38     int           nBytes = 0;
  39
  40     m_name = "MMA7455";
  41
  42     m_controlAddr = devAddr;
  43     m_bus = bus;
  44
  45     m_i2ControlCtx = maa_i2c_init(m_bus);
  46
  47     maa_result_t error = maa_i2c_address(m_i2ControlCtx, m_controlAddr);
  48     if (error != MAA_SUCCESS) {
  49         fprintf(stderr, "Messed up i2c bus\n");
  50         return;
  51     }
  52
  53     // setting GLVL 0x1 (64LSB/g) and MODE 0x1 (Measurement Mode)
  54     data = (BIT (MMA7455_GLVL0) | BIT (MMA7455_MODE0));
  55     error = ic2WriteReg (MMA7455_MCTL, &data, 0x1);
  56     if (error != MAA_SUCCESS) {
  57         std::cout << "ERROR :: MMA7455 instance wan not created (Mode)" << std::endl;
  58         return;
  59     }
  60
  61     if (MAA_SUCCESS != calibrate ()) {
  62         std::cout << "ERROR :: MMA7455 instance wan not created (Calibrate)" << std::endl;
  63         return;
  64     }
  65 }
  66
  67 MMA7455::~MMA7455() {
  68     maa_i2c_stop(m_i2ControlCtx);
  69 }
  70
  71 maa_result_t
  72 MMA7455::calibrate () {
  73     maa_result_t error = MAA_SUCCESS;
  74     int i = 0;
  75
  76     accelData xyz;
  77     xyz.value.x = xyz.value.y = xyz.value.z = 0;
  78
  79     do {
  80         error = readData (&xyz.value.x, &xyz.value.y, &xyz.value.z);
  81         if (MAA_SUCCESS != error) {
  82             return error;
  83         }
  84
  85         xyz.value.x += 2 * -xyz.value.x;
  86         xyz.value.y += 2 * -xyz.value.y;
  87         xyz.value.z += 2 * -(xyz.value.z - 64);
  88
  89         error = ic2WriteReg (MMA7455_XOFFL,  (unsigned char *) &xyz, 0x6);
  90         if (error != MAA_SUCCESS) {
  91             return error;
  92         }
  93
  94     } while ( ++i < 3 );
  95
  96     return error;
  97 }
  98
  99 maa_result_t
 100 MMA7455::readData (short * ptrX, short * ptrY, short * ptrZ) {
 101     accelData xyz;
 102     unsigned char data = 0;
 103     int nBytes = 0;
 104
 105     /*do {
 106         nBytes = ic2ReadReg (MMA7455_STATUS, &data, 0x1);
 107     } while ( !(data & MMA7455_DRDY) && nBytes == MAA_SUCCESS);
 108
 109     if (nBytes == MAA_SUCCESS) {
 110         std::cout << "NO_GDB :: 1" << std::endl;
 111         return MAA_SUCCESS;
 112     }*/
 113
 114     nBytes = ic2ReadReg (MMA7455_XOUTL, (unsigned char *) &xyz, 0x6);
 115     if (nBytes == 0) {
 116         std::cout << "NO_GDB :: 2" << std::endl;
 117         return MAA_ERROR_UNSPECIFIED;
 118     }
 119
 120     if (xyz.reg.x_msb & 0x02) {
 121         xyz.reg.x_msb |= 0xFC;
 122     }
 123
 124     if (xyz.reg.y_msb & 0x02) {
 125         xyz.reg.y_msb |= 0xFC;
 126     }
 127
 128     if (xyz.reg.z_msb & 0x02) {
 129         xyz.reg.z_msb |= 0xFC;
 130     }
 131
 132     // The result is the g-force in units of 64 per 'g'.
 133     *ptrX = xyz.value.x;
 134     *ptrY = xyz.value.y;
 135     *ptrZ = xyz.value.z;
 136
 137     return MAA_SUCCESS;
 138 }
 139
 140 int
 141 MMA7455::ic2ReadReg (unsigned char reg, unsigned char * buf, unsigned char size) {
 142     if (MAA_SUCCESS != maa_i2c_address(m_i2ControlCtx, m_controlAddr)) {
 143         return 0;
 144     }
 145
 146     if (MAA_SUCCESS != maa_i2c_write_byte(m_i2ControlCtx, reg)) {
 147         return 0;
 148     }
 149
 150     if (MAA_SUCCESS != maa_i2c_address(m_i2ControlCtx, m_controlAddr)) {
 151         return 0;
 152     }
 153
 154     return (int) maa_i2c_read(m_i2ControlCtx, buf, size);
 155 }
 156
 157 maa_result_t
 158 MMA7455::ic2WriteReg (unsigned char reg, unsigned char * buf, unsigned char size) {
 159     maa_result_t error = MAA_SUCCESS;
 160
 161     uint8_t data[size + 1];
 162     data[0] = reg;
 163     memcpy(&data[1], buf, size);
 164
 165     error = maa_i2c_address (m_i2ControlCtx, m_controlAddr);
 166     if (error != MAA_SUCCESS) {
 167         return error;
 168     }
 169     error = maa_i2c_write (m_i2ControlCtx, data, size + 1);
 170     if (error != MAA_SUCCESS) {
 171         return error;
 172     }
 173
 174     return error;
 175 }
 176