1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Jon Trulson <jtrulson@ics.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2015 Intel Corporation. 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <iostream> 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <unistd.h> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <stdlib.h> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "mpu9150.h" 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace std; 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 34135d01d28eb401a00da13fbe6bbae82334dc64beBruce BeareMPU9150::MPU9150 (int bus, int address, int magAddress, bool enableAk8975) : 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_mag(0), MPU60X0(bus, address) 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_magAddress = magAddress; 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2cBus = bus; 39135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare m_enableAk8975 = enableAk8975; 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangMPU9150::~MPU9150() 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_mag) 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang delete m_mag; 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU9150::init() 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // init the gyro/accel component 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!MPU60X0::init()) 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": Unable to init MPU60X0"); 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 58135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare // Enabling I2C bypass will allow us to access the 59135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare // AK8975 Magnetometer on I2C addr 0x0c. 60135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare if (m_enableAk8975 == true) 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 62135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare if (!enableI2CBypass(true)) 63135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare { 64135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare throw std::runtime_error(std::string(__FUNCTION__) + 65135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare ": Unable to enable I2C bypass"); 66135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare return false; 67135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare } 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 69135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare // Now that we've done that, create an AK8975 instance and 70135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare // initialize it. 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 72135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare m_mag = new AK8975(m_i2cBus, m_magAddress); 73135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare 74135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare if (!m_mag->init()) 75135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare { 76135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare throw std::runtime_error(std::string(__FUNCTION__) + 77135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare ": Unable to init magnetometer"); 78135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare delete m_mag; 79135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare m_mag = 0; 80135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare return false; 81135d01d28eb401a00da13fbe6bbae82334dc64beBruce Beare } 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU9150::update() 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang MPU60X0::update(); 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_mag) 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_mag->update(); 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU9150::getMagnetometer(float *x, float *y, float *z) 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang float mx, my, mz; 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!m_mag) 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mx = my = mz = 0.0; 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_mag->getMagnetometer(&mx, &my, &mz); 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (x) 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *x = mx; 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (y) 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *y = my; 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (z) 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *z = mz; 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#ifdef SWIGJAVA 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangfloat *MPU9150::getMagnetometer() 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang float *v = new float[3]; 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang getMagnetometer(&v[0], &v[1], &v[2]); 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return v; 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#endif 120