Home | History | Annotate | Download | only in mpu9150 
      1 /*
      2  * Author: Jon Trulson <jtrulson (at) ics.com>
      3  * Copyright (c) 2015 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 
     29 #include "mpu9150.h"
     30 
     31 using namespace upm;
     32 using namespace std;
     33 
     34 MPU9150::MPU9150 (int bus, int address, int magAddress, bool enableAk8975) :
     35   m_mag(0), MPU60X0(bus, address)
     36 {
     37   m_magAddress = magAddress;
     38   m_i2cBus = bus;
     39   m_enableAk8975 = enableAk8975;
     40 }
     41 
     42 MPU9150::~MPU9150()
     43 {
     44   if (m_mag)
     45     delete m_mag;
     46 }
     47 
     48 bool MPU9150::init()
     49 {
     50   // init the gyro/accel component
     51   if (!MPU60X0::init())
     52     {
     53       throw std::runtime_error(std::string(__FUNCTION__) +
     54                                ": Unable to init MPU60X0");
     55       return false;
     56     }
     57 
     58   // Enabling I2C bypass will allow us to access the
     59   // AK8975 Magnetometer on I2C addr 0x0c.
     60   if (m_enableAk8975 == true)
     61     {
     62       if (!enableI2CBypass(true))
     63         {
     64           throw std::runtime_error(std::string(__FUNCTION__) +
     65                                    ": Unable to enable I2C bypass");
     66           return false;
     67         }
     68 
     69       // Now that we've done that, create an AK8975 instance and
     70       // initialize it.
     71 
     72       m_mag = new AK8975(m_i2cBus, m_magAddress);
     73 
     74       if (!m_mag->init())
     75         {
     76           throw std::runtime_error(std::string(__FUNCTION__) +
     77                                  ": Unable to init magnetometer");
     78           delete m_mag;
     79           m_mag = 0;
     80           return false;
     81         }
     82     }
     83 
     84   return true;
     85 }
     86 
     87 void MPU9150::update()
     88 {
     89   MPU60X0::update();
     90 
     91   if (m_mag)
     92     m_mag->update();
     93 }
     94 
     95 void MPU9150::getMagnetometer(float *x, float *y, float *z)
     96 {
     97   float mx, my, mz;
     98 
     99   if (!m_mag)
    100     mx = my = mz = 0.0;
    101   else
    102     m_mag->getMagnetometer(&mx, &my, &mz);
    103 
    104   if (x)
    105     *x = mx;
    106   if (y)
    107     *y = my;
    108   if (z)
    109     *z = mz;
    110 }
    111 
    112 #ifdef SWIGJAVA
    113 float *MPU9150::getMagnetometer()
    114 {
    115     float *v = new float[3];
    116     getMagnetometer(&v[0], &v[1], &v[2]);
    117     return v;
    118 }
    119 #endif
    120