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;
00255 
00256     if (m_use_irq == true && bmi160_irq_asserted == false)
00257         return -1;
00258 
00259     rtnVal = readBlock(DATA_14, DATA_19, localData);
00260     bmi160_irq_asserted = false;
00261     if(rtnVal == RTN_NO_ERROR)
00262     {
00263         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00264         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00265         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00266 
00267         switch(range)
00268         {
00269             case SENS_2G:
00270                 data.xAxis.scaled = (data.xAxis.raw/SENS_2G_LSB_PER_G);
00271                 data.yAxis.scaled = (data.yAxis.raw/SENS_2G_LSB_PER_G);
00272                 data.zAxis.scaled = (data.zAxis.raw/SENS_2G_LSB_PER_G);
00273             break;
00274 
00275             case SENS_4G:
00276                 data.xAxis.scaled = (data.xAxis.raw/SENS_4G_LSB_PER_G);
00277                 data.yAxis.scaled = (data.yAxis.raw/SENS_4G_LSB_PER_G);
00278                 data.zAxis.scaled = (data.zAxis.raw/SENS_4G_LSB_PER_G);
00279             break;
00280 
00281             case SENS_8G:
00282                 data.xAxis.scaled = (data.xAxis.raw/SENS_8G_LSB_PER_G);
00283                 data.yAxis.scaled = (data.yAxis.raw/SENS_8G_LSB_PER_G);
00284                 data.zAxis.scaled = (data.zAxis.raw/SENS_8G_LSB_PER_G);
00285             break;
00286 
00287             case SENS_16G:
00288                 data.xAxis.scaled = (data.xAxis.raw/SENS_16G_LSB_PER_G);
00289                 data.yAxis.scaled = (data.yAxis.raw/SENS_16G_LSB_PER_G);
00290                 data.zAxis.scaled = (data.zAxis.raw/SENS_16G_LSB_PER_G);
00291             break;
00292         }
00293     }
00294 
00295     return rtnVal;
00296 }
00297 
00298 
00299 //*****************************************************************************
00300 int32_t BMI160::getSensorXYZ(SensorData &data, GyroRange range)
00301 {
00302     uint8_t localData[6];
00303     int32_t rtnVal = readBlock(DATA_8, DATA_13, localData);
00304 
00305     if(rtnVal == RTN_NO_ERROR)
00306     {
00307         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00308         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00309         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00310 
00311         switch(range)
00312         {
00313             case DPS_2000:
00314                 data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00315                 data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00316                 data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00317             break;
00318 
00319             case DPS_1000:
00320                 data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00321                 data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00322                 data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00323             break;
00324 
00325             case DPS_500:
00326                 data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00327                 data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00328                 data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00329             break;
00330 
00331             case DPS_250:
00332                 data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00333                 data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00334                 data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00335             break;
00336 
00337             case DPS_125:
00338                 data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00339                 data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00340                 data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00341             break;
00342         }
00343     }
00344 
00345     return rtnVal;
00346 }
00347 
00348 
00349 //*****************************************************************************
00350 int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
00351                                           SensorTime &sensorTime,
00352                                           AccRange range)
00353 {
00354     uint8_t localData[9];
00355     int32_t rtnVal = readBlock(DATA_14, SENSORTIME_2, localData);
00356     if(rtnVal == RTN_NO_ERROR)
00357     {
00358         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00359         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00360         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00361 
00362         switch(range)
00363         {
00364             case SENS_2G:
00365                 data.xAxis.scaled = (data.xAxis.raw/SENS_2G_LSB_PER_G);
00366                 data.yAxis.scaled = (data.yAxis.raw/SENS_2G_LSB_PER_G);
00367                 data.zAxis.scaled = (data.zAxis.raw/SENS_2G_LSB_PER_G);
00368             break;
00369 
00370             case SENS_4G:
00371                 data.xAxis.scaled = (data.xAxis.raw/SENS_4G_LSB_PER_G);
00372                 data.yAxis.scaled = (data.yAxis.raw/SENS_4G_LSB_PER_G);
00373                 data.zAxis.scaled = (data.zAxis.raw/SENS_4G_LSB_PER_G);
00374             break;
00375 
00376             case SENS_8G:
00377                 data.xAxis.scaled = (data.xAxis.raw/SENS_8G_LSB_PER_G);
00378                 data.yAxis.scaled = (data.yAxis.raw/SENS_8G_LSB_PER_G);
00379                 data.zAxis.scaled = (data.zAxis.raw/SENS_8G_LSB_PER_G);
00380             break;
00381 
00382             case SENS_16G:
00383                 data.xAxis.scaled = (data.xAxis.raw/SENS_16G_LSB_PER_G);
00384                 data.yAxis.scaled = (data.yAxis.raw/SENS_16G_LSB_PER_G);
00385                 data.zAxis.scaled = (data.zAxis.raw/SENS_16G_LSB_PER_G);
00386             break;
00387         }
00388 
00389         sensorTime.raw = ((localData[8] << 16) | (localData[7] << 8) |
00390                            localData[6]);
00391         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00392     }
00393 
00394     return rtnVal;
00395 }
00396 
00397 
00398 //*****************************************************************************
00399 int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
00400                                           SensorTime &sensorTime,
00401                                           GyroRange range)
00402 {
00403     uint8_t localData[16];
00404     int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
00405     if(rtnVal == RTN_NO_ERROR)
00406     {
00407         data.xAxis.raw = ((localData[1] << 8) | localData[0]);
00408         data.yAxis.raw = ((localData[3] << 8) | localData[2]);
00409         data.zAxis.raw = ((localData[5] << 8) | localData[4]);
00410 
00411         switch(range)
00412         {
00413             case DPS_2000:
00414                 data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00415                 data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00416                 data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00417             break;
00418 
00419             case DPS_1000:
00420                 data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00421                 data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00422                 data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00423             break;
00424 
00425             case DPS_500:
00426                 data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00427                 data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00428                 data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00429             break;
00430 
00431             case DPS_250:
00432                 data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00433                 data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00434                 data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00435             break;
00436 
00437             case DPS_125:
00438                 data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00439                 data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00440                 data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00441             break;
00442         }
00443 
00444         sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
00445                            localData[12]);
00446         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00447     }
00448 
00449     return rtnVal;
00450 }
00451 
00452 
00453 //*****************************************************************************
00454 int32_t BMI160::getGyroAccXYZandSensorTime(SensorData &accData,
00455                                            SensorData &gyroData,
00456                                            SensorTime &sensorTime,
00457                                            AccRange accRange,
00458                                            GyroRange gyroRange)
00459 {
00460     uint8_t localData[16];
00461     int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
00462     if(rtnVal == RTN_NO_ERROR)
00463     {
00464         gyroData.xAxis.raw = ((localData[1] << 8) | localData[0]);
00465         gyroData.yAxis.raw = ((localData[3] << 8) | localData[2]);
00466         gyroData.zAxis.raw = ((localData[5] << 8) | localData[4]);
00467 
00468         accData.xAxis.raw = ((localData[7] << 8) | localData[6]);
00469         accData.yAxis.raw = ((localData[9] << 8) | localData[8]);
00470         accData.zAxis.raw = ((localData[11] << 8) | localData[10]);
00471 
00472         switch(gyroRange)
00473         {
00474             case DPS_2000:
00475                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00476                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00477                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
00478             break;
00479 
00480             case DPS_1000:
00481                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00482                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00483                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
00484             break;
00485 
00486             case DPS_500:
00487                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00488                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00489                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
00490             break;
00491 
00492             case DPS_250:
00493                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00494                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00495                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
00496             break;
00497 
00498             case DPS_125:
00499                 gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00500                 gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00501                 gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
00502             break;
00503         }
00504 
00505         switch(accRange)
00506         {
00507             case SENS_2G:
00508                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_2G_LSB_PER_G);
00509                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_2G_LSB_PER_G);
00510                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_2G_LSB_PER_G);
00511             break;
00512 
00513             case SENS_4G:
00514                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_4G_LSB_PER_G);
00515                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_4G_LSB_PER_G);
00516                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_4G_LSB_PER_G);
00517             break;
00518 
00519             case SENS_8G:
00520                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_8G_LSB_PER_G);
00521                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_8G_LSB_PER_G);
00522                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_8G_LSB_PER_G);
00523             break;
00524 
00525             case SENS_16G:
00526                 accData.xAxis.scaled = (accData.xAxis.raw/SENS_16G_LSB_PER_G);
00527                 accData.yAxis.scaled = (accData.yAxis.raw/SENS_16G_LSB_PER_G);
00528                 accData.zAxis.scaled = (accData.zAxis.raw/SENS_16G_LSB_PER_G);
00529             break;
00530         }
00531 
00532         sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
00533                            localData[12]);
00534         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00535     }
00536 
00537     return rtnVal;
00538 }
00539 
00540 int32_t BMI160::setSampleRate(int sample_rate)
00541 {
00542     int sr_reg_val = -1;
00543     int i;
00544     const uint16_t odr_table[][2] = {
00545         {25, GYRO_ODR_6}, ///<25Hz
00546         {50, GYRO_ODR_7}, ///<50Hz
00547         {100, GYRO_ODR_8}, ///<100Hz
00548         {200, GYRO_ODR_9}, ///<200Hz
00549         {400, GYRO_ODR_10}, ///<400Hz
00550         {800, GYRO_ODR_11}, ///<800Hz
00551         {1600, GYRO_ODR_12}, ///<1600Hz
00552         {3200, GYRO_ODR_13}, ///<3200Hz
00553     };
00554 
00555     int num_sr = sizeof(odr_table)/sizeof(odr_table[0]);
00556     for (i = 0; i < num_sr; i++) {
00557         if (sample_rate == odr_table[i][0]) {
00558             sr_reg_val = odr_table[i][1];
00559             break;
00560         }
00561     }
00562 
00563     if (sr_reg_val == -1)
00564         return -2;
00565 
00566     AccConfig accConfigRead;
00567     if (getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR) {
00568     accConfigRead.odr = (AccOutputDataRate)sr_reg_val;
00569         return setSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR ? 0 : -1;
00570     } else
00571         return -1;
00572 }
00573 
00574 
00575 //*****************************************************************************
00576 int32_t BMI160::getSensorTime(SensorTime &sensorTime)
00577 {
00578     uint8_t localData[3];
00579     int32_t rtnVal = readBlock(SENSORTIME_0, SENSORTIME_2, localData);
00580 
00581     if(rtnVal == RTN_NO_ERROR)
00582     {
00583         sensorTime.raw = ((localData[2] << 16) | (localData[1] << 8) |
00584                            localData[0]);
00585         sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
00586     }
00587 
00588     return rtnVal;
00589 }
00590 
00591 
00592 //*****************************************************************************
00593 int32_t BMI160::getTemperature(float *temp)
00594 {
00595     uint8_t data[2];
00596     uint16_t rawTemp;
00597 
00598     int32_t rtnVal = readBlock(TEMPERATURE_0, TEMPERATURE_1, data);
00599     if(rtnVal == RTN_NO_ERROR)
00600     {
00601         rawTemp = ((data[1] << 8) | data[0]);
00602         if(rawTemp & 0x8000)
00603         {
00604             *temp = (23.0F - ((0x10000 - rawTemp)/512.0F));
00605         }
00606         else
00607         {
00608             *temp = ((rawTemp/512.0F) + 23.0F);
00609         }
00610     }
00611 
00612     return rtnVal;
00613 }
00614 
00615 //***********************************************************************************
00616 int32_t BMI160::BMI160_DefaultInitalize(){
00617 
00618         //soft reset the accelerometer
00619         writeRegister(CMD ,SOFT_RESET);
00620         wait(0.1);
00621 
00622         //Power up sensors in normal mode
00623         if(setSensorPowerMode(BMI160::GYRO, BMI160::SUSPEND) != BMI160::RTN_NO_ERROR){
00624             printf("Failed to set gyroscope power mode\n");
00625         }
00626 
00627         wait(0.1);
00628 
00629         if(setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR){
00630             printf("Failed to set accelerometer power mode\n");
00631         }
00632         wait(0.1);
00633 
00634         BMI160::AccConfig accConfig;
00635         BMI160::AccConfig accConfigRead;
00636         accConfig.range = BMI160::SENS_2G;
00637         accConfig.us = BMI160::ACC_US_OFF;
00638         accConfig.bwp = BMI160::ACC_BWP_2;
00639         accConfig.odr = BMI160::ACC_ODR_6;
00640         if(setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
00641         {
00642             if(getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
00643             {
00644                 if((accConfig.range != accConfigRead.range) ||
00645                         (accConfig.us != accConfigRead.us) ||
00646                         (accConfig.bwp != accConfigRead.bwp) ||
00647                         (accConfig.odr != accConfigRead.odr))
00648                 {
00649                     printf("ACC read data desn't equal set data\n\n");
00650                     printf("ACC Set Range = %d\n", accConfig.range);
00651                     printf("ACC Set UnderSampling = %d\n", accConfig.us);
00652                     printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
00653                     printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
00654                     printf("ACC Read Range = %d\n", accConfigRead.range);
00655                     printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
00656                     printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
00657                     printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
00658                 }
00659 
00660             }
00661             else
00662             {
00663                  printf("Failed to read back accelerometer configuration\n");
00664             }
00665         }
00666         else
00667         {
00668             printf("Failed to set accelerometer configuration\n");
00669         }
00670         return 0;
00671 }
00672 
00673 //***********************************************************************************
00674 int32_t BMI160::enable_data_ready_interrupt() {
00675     uint8_t data = 0;
00676     uint8_t temp = 0;
00677     int32_t result;
00678 
00679     result = readRegister(INT_EN_1, &data);
00680     temp = data & ~0x10;
00681     data = temp | ((1 << 4) & 0x10);
00682     /* Writing data to INT ENABLE 1 Address */
00683     result |= writeRegister(INT_EN_1, data);
00684 
00685     // configure in_out ctrl
00686     //bmi160_get_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev);
00687     result |= readRegister(INT_OUT_CTRL, &data);
00688     data = 0x09;
00689     result |= writeRegister(INT_OUT_CTRL,data);
00690 
00691     //config int latch
00692     //bmi160_get_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev);
00693     result |= readRegister(INT_LATCH, &data);
00694     data = 0x0F;
00695     result |= writeRegister(INT_LATCH, data);
00696 
00697     //bmi160_get_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev);
00698     result |= readRegister(INT_MAP_1, &data);
00699     data = 0x80;
00700     result |= writeRegister(INT_MAP_1, data);
00701 
00702     if(result != 0){
00703         printf("BMI160::%s failed.\r\n", __func__);
00704         return -1;
00705     }
00706 
00707     m_bmi160_irq->disable_irq();
00708     m_bmi160_irq->mode(PullUp);
00709     m_bmi160_irq->fall(this, &BMI160::irq_handler);
00710     m_bmi160_irq->enable_irq();
00711     return 0;
00712 }
00713 
00714 void BMI160::irq_handler() {
00715     bmi160_irq_asserted = true;
00716 }
00717 
00718 int32_t BMI160::reset() {
00719     if (m_use_irq)
00720         m_bmi160_irq->disable_irq();
00721     bmi160_irq_asserted = false;
00722     writeRegister(CMD, SOFT_RESET);
00723     return 0;
00724 }