bmi160.cpp

00001 /**********************************************************************
00002 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
00003 *
00004 * Permission is hereby granted, free of charge, to any person obtaining a
00005 * copy of this software and associated documentation files (the "Software"),
00006 * to deal in the Software without restriction, including without limitation
00007 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00008 * and/or sell copies of the Software, and to permit persons to whom the
00009 * Software is furnished to do so, subject to the following conditions:
00010 *
00011 * The above copyright notice and this permission notice shall be included
00012 * in all copies or substantial portions of the Software.
00013 *
00014 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00018 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00019 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00020 * OTHER DEALINGS IN THE SOFTWARE.
00021 *
00022 * Except as contained in this notice, the name of Maxim Integrated
00023 * Products, Inc. shall not be used except as stated in the Maxim Integrated
00024 * Products, Inc. Branding Policy.
00025 *
00026 * The mere transfer of this software does not imply any licenses
00027 * of trade secrets, proprietary technology, copyrights, patents,
00028 * trademarks, maskwork rights, or any other form of intellectual
00029 * property whatsoever. Maxim Integrated Products, Inc. retains all
00030 * ownership rights.
00031 **********************************************************************/
00032 
00033 
00034 #include "bmi160.h"
00035 
00036 
00037 const struct BMI160::AccConfig BMI160::DEFAULT_ACC_CONFIG = {SENS_2G,
00038                                                              ACC_US_OFF,
00039                                                              ACC_BWP_2,
00040                                                              ACC_ODR_8};
00041 
00042 const struct BMI160::GyroConfig BMI160::DEFAULT_GYRO_CONFIG = {DPS_2000,
00043                                                                GYRO_BWP_2,
00044                                                                GYRO_ODR_8};
00045 
00046 
00047 //*****************************************************************************
00048 int32_t BMI160::setSensorPowerMode(Sensors sensor, PowerModes pwrMode)
00049 {
00050     int32_t rtnVal = -1;
00051 
00052     switch(sensor)
00053     {
00054         case MAG:
00055             rtnVal = writeRegister(CMD, (MAG_SET_PMU_MODE | pwrMode));
00056         break;
00057 
00058         case GYRO:
00059             rtnVal = writeRegister(CMD, (GYR_SET_PMU_MODE | pwrMode));
00060         break;
00061 
00062         case ACC:
00063             rtnVal = writeRegister(CMD, (ACC_SET_PMU_MODE | pwrMode));
00064         break;
00065 
00066         default:
00067             rtnVal = -1;
00068         break;
00069     }
00070 
00071     return rtnVal;
00072 }
00073 
00074 
00075 //*****************************************************************************
00076 int32_t BMI160::setSensorConfig(const AccConfig &config)
00077 {
00078     uint8_t data[2];
00079 
00080     data[0] = ((config.us << ACC_US_POS) | (config.bwp << ACC_BWP_POS) |
00081                (config.odr << ACC_ODR_POS));
00082     data[1] = config.range;
00083 
00084     return writeBlock(ACC_CONF, ACC_RANGE, data);
00085 }
00086 
00087 
00088 //*****************************************************************************
00089 int32_t BMI160::setSensorConfig(const GyroConfig &config)
00090 {
00091     uint8_t data[2];
00092 
00093     data[0] = ((config.bwp << GYRO_BWP_POS) | (config.odr << GYRO_ODR_POS));
00094     data[1] = config.range;
00095 
00096     return writeBlock(GYR_CONF, GYR_RANGE, data);
00097 }
00098 
00099 
00100 //*****************************************************************************
00101 int32_t BMI160::getSensorConfig(AccConfig &config)
00102 {
00103     uint8_t data[2];
00104     int32_t rtnVal = readBlock(ACC_CONF, ACC_RANGE, data);
00105 
00106     if(rtnVal == RTN_NO_ERROR)
00107     {
00108         config.range = static_cast<BMI160::AccRange>(
00109         (data[1] & ACC_RANGE_MASK));
00110         config.us = static_cast<BMI160::AccUnderSampling>(
00111         ((data[0] & ACC_US_MASK) >> ACC_US_POS));
00112         config.bwp = static_cast<BMI160::AccBandWidthParam>(
00113         ((data[0] & ACC_BWP_MASK) >> ACC_BWP_POS));
00114         config.odr = static_cast<BMI160::AccOutputDataRate>(
00115         ((data[0] & ACC_ODR_MASK) >> ACC_ODR_POS));
00116     }
00117 
00118     return rtnVal;
00119 }
00120 
00121 
00122 //*****************************************************************************
00123 int32_t BMI160::getSensorConfig(GyroConfig &config)
00124 {
00125     uint8_t data[2];
00126     int32_t rtnVal = readBlock(GYR_CONF, GYR_RANGE, data);
00127 
00128     if(rtnVal == RTN_NO_ERROR)
00129     {
00130         config.range = static_cast<BMI160::GyroRange>(
00131         (data[1] & GYRO_RANGE_MASK));
00132         config.bwp = static_cast<BMI160::GyroBandWidthParam>(
00133         ((data[0] & GYRO_BWP_MASK) >> GYRO_BWP_POS));
00134         config.odr = static_cast<BMI160::GyroOutputDataRate>(
00135         ((data[0] & GYRO_ODR_MASK) >> GYRO_ODR_POS));
00136     }
00137 
00138     return rtnVal;
00139 }
00140 
00141 
00142 //*****************************************************************************
00143 int32_t BMI160::getSensorAxis(SensorAxis axis, AxisData &data, AccRange range)
00144 {
00145     uint8_t localData[2];
00146     int32_t rtnVal;
00147 
00148     switch(axis)
00149     {
00150         case X_AXIS:
00151             rtnVal = readBlock(DATA_14, DATA_15, localData);
00152         break;
00153 
00154         case Y_AXIS:
00155             rtnVal = readBlock(DATA_16, DATA_17, localData);
00156         break;
00157 
00158         case Z_AXIS:
00159             rtnVal = readBlock(DATA_18, DATA_19, localData);
00160         break;
00161 
00162         default:
00163             rtnVal = -1;
00164         break;
00165     }
00166 
00167     if(rtnVal == RTN_NO_ERROR)
00168     {
00169         data.raw = ((localData[1] << 8) | localData[0]);
00170         switch(range)
00171         {
00172             case SENS_2G:
00173                 data.scaled = (data.raw/SENS_2G_LSB_PER_G);
00174             break;
00175 
00176             case SENS_4G:
00177                 data.scaled = (data.raw/SENS_4G_LSB_PER_G);
00178             break;
00179 
00180             case SENS_8G:
00181                 data.scaled = (data.raw/SENS_8G_LSB_PER_G);
00182             break;
00183 
00184             case SENS_16G:
00185                 data.scaled = (data.raw/SENS_16G_LSB_PER_G);
00186             break;
00187         }
00188     }
00189 
00190     return rtnVal;
00191 }
00192 
00193 
00194 //*****************************************************************************
00195 int32_t BMI160::getSensorAxis(SensorAxis axis, AxisData &data, GyroRange range)
00196 {
00197     uint8_t localData[2];
00198     int32_t rtnVal;
00199 
00200     switch(axis)
00201     {
00202         case X_AXIS:
00203             rtnVal = readBlock(DATA_8, DATA_9, localData);
00204         break;
00205 
00206         case Y_AXIS:
00207             rtnVal = readBlock(DATA_10, DATA_11, localData);
00208         break;
00209 
00210         case Z_AXIS:
00211             rtnVal = readBlock(DATA_12, DATA_13, localData);
00212         break;
00213 
00214         default:
00215             rtnVal = -1;
00216         break;
00217     }
00218 
00219     if(rtnVal == RTN_NO_ERROR)
00220     {
00221         data.raw = ((localData[1] << 8) | localData[0]);
00222         switch(range)
00223         {
00224             case DPS_2000:
00225                 data.scaled = (data.raw/SENS_2000_DPS_LSB_PER_DPS);
00226             break;
00227 
00228             case DPS_1000:
00229                 data.scaled = (data.raw/SENS_1000_DPS_LSB_PER_DPS);
00230             break;
00231 
00232             case DPS_500:
00233                 data.scaled = (data.raw/SENS_500_DPS_LSB_PER_DPS);
00234             break;
00235 
00236             case DPS_250:
00237                 data.scaled = (data.raw/SENS_250_DPS_LSB_PER_DPS);
00238             break;
00239 
00240             case DPS_125:
00241                 data.scaled = (data.raw/SENS_125_DPS_LSB_PER_DPS);
00242             break;
00243         }
00244     }
00245 
00246     return rtnVal;
00247 }
00248 
00249 
00250 //*****************************************************************************
00251 int32_t BMI160::getSensorXYZ(SensorData &data, AccRange range)
00252 {
00253     uint8_t localData[6];
00254     int32_t rtnVal = readBlock(DATA_14, DATA_19, localData);
00255 
00256     if (m_use_irq == true && bmi160_irq_asserted == false)
00257         return -1;
00258 
00259     bmi160_irq_asserted = false;
00260     if(rtnVal == RTN_NO_ERROR)
00261     {
00262         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00263         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00264         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00265 
00266         switch(range)
00267         {
00268             case SENS_2G:
00269                 data.xAxis.scaled = (data.xAxis.raw/SENS_2G_LSB_PER_G);
00270                 data.yAxis.scaled = (data.yAxis.raw/SENS_2G_LSB_PER_G);
00271                 data.zAxis.scaled = (data.zAxis.raw/SENS_2G_LSB_PER_G);
00272             break;
00273 
00274             case SENS_4G:
00275                 data.xAxis.scaled = (data.xAxis.raw/SENS_4G_LSB_PER_G);
00276                 data.yAxis.scaled = (data.yAxis.raw/SENS_4G_LSB_PER_G);
00277                 data.zAxis.scaled = (data.zAxis.raw/SENS_4G_LSB_PER_G);
00278             break;
00279 
00280             case SENS_8G:
00281                 data.xAxis.scaled = (data.xAxis.raw/SENS_8G_LSB_PER_G);
00282                 data.yAxis.scaled = (data.yAxis.raw/SENS_8G_LSB_PER_G);
00283                 data.zAxis.scaled = (data.zAxis.raw/SENS_8G_LSB_PER_G);
00284             break;
00285 
00286             case SENS_16G:
00287                 data.xAxis.scaled = (data.xAxis.raw/SENS_16G_LSB_PER_G);
00288                 data.yAxis.scaled = (data.yAxis.raw/SENS_16G_LSB_PER_G);
00289                 data.zAxis.scaled = (data.zAxis.raw/SENS_16G_LSB_PER_G);
00290             break;
00291         }
00292     }
00293 
00294     return rtnVal;
00295 }
00296 
00297 
00298 //*****************************************************************************
00299 int32_t BMI160::getSensorXYZ(SensorData &data, GyroRange range)
00300 {
00301     uint8_t localData[6];
00302     int32_t rtnVal = readBlock(DATA_8, DATA_13, localData);
00303 
00304     if(rtnVal == RTN_NO_ERROR)
00305     {
00306         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00307         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00308         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00309 
00310         switch(range)
00311         {
00312             case DPS_2000:
00313                 data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00314                 data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00315                 data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00316             break;
00317 
00318             case DPS_1000:
00319                 data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00320                 data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00321                 data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00322             break;
00323 
00324             case DPS_500:
00325                 data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00326                 data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00327                 data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00328             break;
00329 
00330             case DPS_250:
00331                 data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00332                 data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00333                 data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00334             break;
00335 
00336             case DPS_125:
00337                 data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00338                 data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00339                 data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00340             break;
00341         }
00342     }
00343 
00344     return rtnVal;
00345 }
00346 
00347 
00348 //*****************************************************************************
00349 int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
00350                                           SensorTime &sensorTime,
00351                                           AccRange range)
00352 {
00353     uint8_t localData[9];
00354     int32_t rtnVal = readBlock(DATA_14, SENSORTIME_2, localData);
00355     if(rtnVal == RTN_NO_ERROR)
00356     {
00357         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00358         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00359         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00360 
00361         switch(range)
00362         {
00363             case SENS_2G:
00364                 data.xAxis.scaled = (data.xAxis.raw/SENS_2G_LSB_PER_G);
00365                 data.yAxis.scaled = (data.yAxis.raw/SENS_2G_LSB_PER_G);
00366                 data.zAxis.scaled = (data.zAxis.raw/SENS_2G_LSB_PER_G);
00367             break;
00368 
00369             case SENS_4G:
00370                 data.xAxis.scaled = (data.xAxis.raw/SENS_4G_LSB_PER_G);
00371                 data.yAxis.scaled = (data.yAxis.raw/SENS_4G_LSB_PER_G);
00372                 data.zAxis.scaled = (data.zAxis.raw/SENS_4G_LSB_PER_G);
00373             break;
00374 
00375             case SENS_8G:
00376                 data.xAxis.scaled = (data.xAxis.raw/SENS_8G_LSB_PER_G);
00377                 data.yAxis.scaled = (data.yAxis.raw/SENS_8G_LSB_PER_G);
00378                 data.zAxis.scaled = (data.zAxis.raw/SENS_8G_LSB_PER_G);
00379             break;
00380 
00381             case SENS_16G:
00382                 data.xAxis.scaled = (data.xAxis.raw/SENS_16G_LSB_PER_G);
00383                 data.yAxis.scaled = (data.yAxis.raw/SENS_16G_LSB_PER_G);
00384                 data.zAxis.scaled = (data.zAxis.raw/SENS_16G_LSB_PER_G);
00385             break;
00386         }
00387 
00388         sensorTime.raw = ((localData[8] << 16) | (localData[7] << 8) |
00389                            localData[6]);
00390         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00391     }
00392 
00393     return rtnVal;
00394 }
00395 
00396 
00397 //*****************************************************************************
00398 int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
00399                                           SensorTime &sensorTime,
00400                                           GyroRange range)
00401 {
00402     uint8_t localData[16];
00403     int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
00404     if(rtnVal == RTN_NO_ERROR)
00405     {
00406         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00407         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00408         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00409 
00410         switch(range)
00411         {
00412             case DPS_2000:
00413                 data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00414                 data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00415                 data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00416             break;
00417 
00418             case DPS_1000:
00419                 data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00420                 data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00421                 data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00422             break;
00423 
00424             case DPS_500:
00425                 data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00426                 data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00427                 data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00428             break;
00429 
00430             case DPS_250:
00431                 data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00432                 data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00433                 data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00434             break;
00435 
00436             case DPS_125:
00437                 data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00438                 data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00439                 data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00440             break;
00441         }
00442 
00443         sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
00444                            localData[12]);
00445         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00446     }
00447 
00448     return rtnVal;
00449 }
00450 
00451 
00452 //*****************************************************************************
00453 int32_t BMI160::getGyroAccXYZandSensorTime(SensorData &accData,
00454                                            SensorData &gyroData,
00455                                            SensorTime &sensorTime,
00456                                            AccRange accRange,
00457                                            GyroRange gyroRange)
00458 {
00459     uint8_t localData[16];
00460     int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
00461     if(rtnVal == RTN_NO_ERROR)
00462     {
00463         gyroData.xAxis.raw = ((localData[1] << 8) | localData[0]);
00464         gyroData.yAxis.raw = ((localData[3] << 8) | localData[2]);
00465         gyroData.zAxis.raw = ((localData[5] << 8) | localData[4]);
00466 
00467         accData.xAxis.raw = ((localData[7] << 8) | localData[6]);
00468         accData.yAxis.raw = ((localData[9] << 8) | localData[8]);
00469         accData.zAxis.raw = ((localData[11] << 8) | localData[10]);
00470 
00471         switch(gyroRange)
00472         {
00473             case DPS_2000:
00474                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00475                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00476                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00477             break;
00478 
00479             case DPS_1000:
00480                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00481                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00482                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00483             break;
00484 
00485             case DPS_500:
00486                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00487                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00488                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00489             break;
00490 
00491             case DPS_250:
00492                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00493                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00494                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00495             break;
00496 
00497             case DPS_125:
00498                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00499                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00500                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00501             break;
00502         }
00503 
00504         switch(accRange)
00505         {
00506             case SENS_2G:
00507                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_2G_LSB_PER_G);
00508                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_2G_LSB_PER_G);
00509                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_2G_LSB_PER_G);
00510             break;
00511 
00512             case SENS_4G:
00513                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_4G_LSB_PER_G);
00514                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_4G_LSB_PER_G);
00515                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_4G_LSB_PER_G);
00516             break;
00517 
00518             case SENS_8G:
00519                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_8G_LSB_PER_G);
00520                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_8G_LSB_PER_G);
00521                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_8G_LSB_PER_G);
00522             break;
00523 
00524             case SENS_16G:
00525                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_16G_LSB_PER_G);
00526                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_16G_LSB_PER_G);
00527                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_16G_LSB_PER_G);
00528             break;
00529         }
00530 
00531         sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
00532                            localData[12]);
00533         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00534     }
00535 
00536     return rtnVal;
00537 }
00538 
00539 int32_t BMI160::setSampleRate(int sample_rate)
00540 {
00541     int sr_reg_val = -1;
00542     int i;
00543     const uint16_t odr_table[][2] = {
00544         {25, GYRO_ODR_6}, ///<25Hz
00545         {50, GYRO_ODR_7}, ///<50Hz
00546         {100, GYRO_ODR_8}, ///<100Hz
00547         {200, GYRO_ODR_9}, ///<200Hz
00548         {400, GYRO_ODR_10}, ///<400Hz
00549         {800, GYRO_ODR_11}, ///<800Hz
00550         {1600, GYRO_ODR_12}, ///<1600Hz
00551         {3200, GYRO_ODR_13}, ///<3200Hz
00552     };
00553 
00554     int num_sr = sizeof(odr_table)/sizeof(odr_table[0]);
00555     for (i = 0; i < num_sr; i++) {
00556         if (sample_rate == odr_table[i][0]) {
00557             sr_reg_val = odr_table[i][1];
00558             break;
00559         }
00560     }
00561 
00562     if (sr_reg_val == -1)
00563         return -2;
00564 
00565     AccConfig accConfigRead;
00566     if (getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR) {
00567     accConfigRead.odr = (AccOutputDataRate)sr_reg_val;
00568         return setSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR ? 0 : -1;
00569     } else
00570         return -1;
00571 }
00572 
00573 
00574 //*****************************************************************************
00575 int32_t BMI160::getSensorTime(SensorTime &sensorTime)
00576 {
00577     uint8_t localData[3];
00578     int32_t rtnVal = readBlock(SENSORTIME_0, SENSORTIME_2, localData);
00579 
00580     if(rtnVal == RTN_NO_ERROR)
00581     {
00582         sensorTime.raw = ((localData[2] << 16) | (localData[1] << 8) |
00583                            localData[0]);
00584         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00585     }
00586 
00587     return rtnVal;
00588 }
00589 
00590 
00591 //*****************************************************************************
00592 int32_t BMI160::getTemperature(float *temp)
00593 {
00594     uint8_t data[2];
00595     uint16_t rawTemp;
00596 
00597     int32_t rtnVal = readBlock(TEMPERATURE_0, TEMPERATURE_1, data);
00598     if(rtnVal == RTN_NO_ERROR)
00599     {
00600         rawTemp = ((data[1] << 8) | data[0]);
00601         if(rawTemp & 0x8000)
00602         {
00603             *temp = (23.0F - ((0x10000 - rawTemp)/512.0F));
00604         }
00605         else
00606         {
00607             *temp = ((rawTemp/512.0F) + 23.0F);
00608         }
00609     }
00610 
00611     return rtnVal;
00612 }
00613 
00614 //***********************************************************************************
00615 int32_t BMI160::BMI160_DefaultInitalize(){
00616 
00617         //soft reset the accelerometer
00618         writeRegister(CMD ,SOFT_RESET);
00619         wait(0.1);
00620 
00621         //Power up sensors in normal mode
00622         if(setSensorPowerMode(BMI160::GYRO, BMI160::SUSPEND) != BMI160::RTN_NO_ERROR){
00623             printf("Failed to set gyroscope power mode\n");
00624         }
00625 
00626         wait(0.1);
00627 
00628         if(setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){
00629             printf("Failed to set accelerometer power mode\n");
00630         }
00631         wait(0.1);
00632 
00633         BMI160::AccConfig accConfig;
00634         BMI160::AccConfig accConfigRead;
00635         accConfig.range = BMI160::SENS_2G;
00636         accConfig.us = BMI160::ACC_US_OFF;
00637         accConfig.bwp = BMI160::ACC_BWP_2;
00638         accConfig.odr = BMI160::ACC_ODR_6;
00639         if(setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
00640         {
00641             if(getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
00642             {
00643                 if((accConfig.range != accConfigRead.range) ||
00644                         (accConfig.us != accConfigRead.us) ||
00645                         (accConfig.bwp != accConfigRead.bwp) ||
00646                         (accConfig.odr != accConfigRead.odr))
00647                 {
00648                     printf("ACC read data desn't equal set data\n\n");
00649                     printf("ACC Set Range = %d\n", accConfig.range);
00650                     printf("ACC Set UnderSampling = %d\n", accConfig.us);
00651                     printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
00652                     printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
00653                     printf("ACC Read Range = %d\n", accConfigRead.range);
00654                     printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
00655                     printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
00656                     printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
00657                 }
00658 
00659             }
00660             else
00661             {
00662                  printf("Failed to read back accelerometer configuration\n");
00663             }
00664         }
00665         else
00666         {
00667             printf("Failed to set accelerometer configuration\n");
00668         }
00669         return 0;
00670 }
00671 
00672 //***********************************************************************************
00673 int32_t BMI160::enable_data_ready_interrupt() {
00674     uint8_t data = 0;
00675     uint8_t temp = 0;
00676     int32_t result;
00677 
00678     result = readRegister(INT_EN_1, &data);
00679     temp = data & ~0x10;
00680     data = temp | ((1 << 4) & 0x10);
00681     /* Writing data to INT ENABLE 1 Address */
00682     result |= writeRegister(INT_EN_1, data);
00683 
00684     // configure in_out ctrl
00685     //bmi160_get_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev);
00686     result |= readRegister(INT_OUT_CTRL, &data);
00687     data = 0x09;
00688     result |= writeRegister(INT_OUT_CTRL,data);
00689 
00690     //config int latch
00691     //bmi160_get_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev);
00692     result |= readRegister(INT_LATCH, &data);
00693     data = 0x0F;
00694     result |= writeRegister(INT_LATCH, data);
00695 
00696     //bmi160_get_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev);
00697     result |= readRegister(INT_MAP_1, &data);
00698     data = 0x80;
00699     result |= writeRegister(INT_MAP_1, data);
00700 
00701     if(result != 0){
00702         printf("BMI160::%s failed.\r\n", __func__);
00703         return -1;
00704     }
00705 
00706     m_bmi160_irq->disable_irq();
00707     m_bmi160_irq->mode(PullUp);
00708     m_bmi160_irq->fall(this, &BMI160::irq_handler);
00709     m_bmi160_irq->enable_irq();
00710     return 0;
00711 }
00712 
00713 void BMI160::irq_handler() {
00714     bmi160_irq_asserted = true;
00715 }
00716 
00717 int32_t BMI160::reset() {
00718     if (m_use_irq)
00719         m_bmi160_irq->disable_irq();
00720     bmi160_irq_asserted = false;
00721     writeRegister(CMD, SOFT_RESET);
00722     return 0;
00723 }