mehmet gok
Utility library for HSP SPo2 HR demo including user interface, board support adn accelerometer.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 01:41:58 by
1.7.2