00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include <iostream>
00052
00053
00054 #include <ros/console.h>
00055
00056 #include "bma180_driver/bma180.hpp"
00057 #include "bma180_driver/bma180_parameters.hpp"
00058
00059
00060
00061
00062
00063 BMA180::BMA180( bosch_hardware_interface* hw ) :
00064 sensor_driver( hw ),
00065 BMA180Parameters()
00066 {
00067
00068 TempSlope_ = 0.5;
00069 }
00070
00071
00072
00073
00074
00075 BMA180::~BMA180()
00076 {
00077 }
00078
00079
00080
00081
00082 uint8_t BMA180::getDeviceAddress()
00083 {
00084
00085 switch( this->getProtocol() )
00086 {
00087 case I2C:
00088
00089 if( this->getSlaveAddressBit() == 0 )
00090 return SLAVE_ADDRESS0;
00091 else
00092 if( this->getSlaveAddressBit() == 1 )
00093 return SLAVE_ADDRESS1;
00094 else
00095 ROS_ERROR( "BMA180::getDeviceAddress(): invalid I2C address" );
00096 break;
00097 case SPI:
00098 return this->getPin();
00099 default:
00100 ROS_ERROR( "BMA180::getDeviceAddress(): sensor has no identification. Either setPin(uint8_t pin) for SPI or setSlaveAddress( 0 or 1) for I2C." );
00101 return 255;
00102 }
00103 ROS_ERROR( "BMA180::getDeviceAddress(): sensor has no identification. Either setPin(uint8_t pin) for SPI or setSlaveAddress( 0 or 1) for I2C." );
00104 return 255;
00105 }
00106
00107
00108
00109
00110
00111 bool BMA180::initialize()
00112 {
00113 ROS_INFO( " " );
00114 ROS_INFO( "BMA180::initialize(): Device Address (hex): %x",this->getDeviceAddress() );
00115 ROS_INFO( "BMA180::initialize(): Protocol: %d",this->getProtocol() );
00116 ROS_INFO( "BMA180::initialize(): Frequency: %d",this->getFrequency() );
00117
00118
00119 if( hardware_->initialize() == false )
00120 return false;
00121
00122
00123 if( this->softReset() == false )
00124 return false;
00125 ROS_INFO( "BMA180::initialize(): soft reset applied." );
00126
00127
00128 switch( this->getProtocol() )
00129 {
00130 case I2C:
00131 break;
00132 case SPI:
00133
00134 if( this->DisableI2C() == false )
00135 return false;
00136 ROS_INFO( "BMA180::initialize(): Disabled I2C mode." );
00137 break;
00138 default:
00139 ROS_ERROR( "BMA180::initialize(): BMA180 cannot be read with selected protocol." );
00140 return false;
00141 }
00142
00143
00144 if( this->EnableWriting() == false )
00145 return false;
00146
00147
00148 if( this->changeAccelRange() == false )
00149 return false;
00150
00151
00152 if( this->changeBandwidth() == false )
00153 return false;
00154
00155 ROS_INFO( "BMA180 initialized." );
00156 sleep( .1 );
00157
00158 return true;
00159 }
00160
00161
00162
00163
00164
00165 bool BMA180::takeMeasurement()
00166 {
00167 uint8_t Data[7];
00168 if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 7 ) == false )
00169 {
00170 ROS_ERROR(" BMA180::takeMeasurement(): Unable to read accelerometer data from sensor." );
00171 return false;
00172 }
00173
00174 AccelX_ = this->getSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2 );
00175 AccelY_ = this->getSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 2 );
00176 AccelZ_ = this->getSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 2 );
00177 Temperature_ = ( TempSlope_* (int8_t)Data[6] ) + 24;
00178 return true;
00179 }
00180
00181
00182
00183
00184 double BMA180::getStaticPitch()
00185 {
00186
00187 StaticPitch_ = atan2( AccelY_, AccelZ_ ) ;
00188 return StaticPitch_;
00189 }
00190
00191
00192
00193
00194 double BMA180::getStaticRoll()
00195 {
00196 StaticRoll_ = atan2( AccelX_, AccelZ_ );
00197 return ( StaticRoll_ );
00198 }
00199
00200
00201
00202
00203
00204 bool BMA180::getAccelData()
00205 {
00206 uint8_t Data[6];
00207 if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 6 ) == false )
00208 {
00209 ROS_ERROR( "BMA180::getAccelData(): Unable to read accelerometer data from sensor." );
00210 return false;
00211 }
00212 AccelX_ = this->getSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2 );
00213 AccelY_ = this->getSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 2 );
00214 AccelZ_ = this->getSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 2 );
00215 return true;
00216 }
00217
00218
00219
00220
00221
00222 double BMA180::getAccelX()
00223 {
00224 uint8_t Data[2];
00225
00226
00227 if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 2 ) == false )
00228 {
00229 ROS_ERROR("BMA180: cannot read from this protocol.");
00230 return -9999.9;
00231 }
00232 int raw_data = int(( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2);
00233 AccelX_ = raw_data * this->getSensitivity();
00234
00235 return AccelX_;
00236 }
00237
00238
00239
00240
00241
00242
00243 double BMA180::getAccelY()
00244 {
00245 uint8_t Data[2];
00246
00247 if( this->readSensorData( ADDRESS_ACCLY_LSB, Data, 2 ) == false )
00248 {
00249 ROS_ERROR("BMA180::getAccelY(): failed.");
00250 return -9999.9;
00251 }
00252 double raw_data = (( ((int16_t)(Data[1] << 8)) | ( (int16_t)(Data[0]) ) ) >> 2);
00253 AccelY_ = raw_data * this->getSensitivity();
00254
00255 return AccelY_;
00256 }
00257
00258
00259
00260
00261
00262 double BMA180::getAccelZ()
00263 {
00264 uint8_t Data[2];
00265 if( this->readSensorData( ADDRESS_ACCLZ_LSB, Data, 2 ) == false )
00266 {
00267 ROS_ERROR( "BMA180: cannot read from this protocol." );
00268 return -9999.9;
00269 }
00270
00271 double raw_data = (( ((int16_t)(Data[1] << 8)) | ( (int16_t)(Data[0]) ) ) >> 2);
00272 AccelZ_ = raw_data * this->getSensitivity();
00273
00274 return AccelZ_;
00275 }
00276
00277
00278
00279
00280
00281 double BMA180::getTemperature()
00282 {
00283 uint8_t Temperature;
00284
00285 if( this->readReg( ADDRESS_TEMPERATURE, &Temperature ) == false )
00286 {
00287 ROS_ERROR("BMA180::getTemperature(): failed.");
00288 return -9999.9;
00289 }
00290
00291 Temperature_ = ( TempSlope_* (int8_t)Temperature ) + 24;
00292 return Temperature_;
00293 }
00294
00295
00296
00297
00298
00299 bool BMA180::softReset()
00300 {
00301
00302 uint8_t Request_SoftReset = (uint8_t)CMD_SOFTRESET;
00303
00304
00305 if( this->writeToReg( ADDRESS_SOFTRESET, Request_SoftReset ) == false )
00306 {
00307 ROS_ERROR("BMA180::softReset(): write failed.");
00308 return false;
00309 }
00310
00311
00312 usleep( 10 );
00313
00314 return true;
00315 }
00316
00317
00318
00319
00320
00321 bool BMA180::Calibrate()
00322 {
00323 uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00324 uint8_t oldOffsets[3];
00325 uint8_t newOffsets[3];
00326
00327 for( int i = 0; i < 3; i++ )
00328 {
00329 ROS_INFO( "BMA180::Calibrate(): BEGIN Calibrating axis %d", i );
00330
00331
00332
00333 if( i == 0 )
00334 {
00335 if( this->readReg( 0x29, &oldOffsets[i] ) == false )
00336 return false;
00337 oldOffsets[i] = (oldOffsets[i] >> 1) & 0x7F;
00338 }
00339
00340
00341 if( i == 1 )
00342 {
00343 if( this->readReg( 0x26, &oldOffsets[i] ) == false)
00344 return false;
00345 oldOffsets[i] = (oldOffsets[i] >> 1) & 0x7F;
00346 }
00347
00348
00349 if( i == 2 )
00350 {
00351 uint8_t temp_reg1;
00352 uint8_t temp_reg2;
00353
00354 if( this->readReg( 0x25, &temp_reg1 ) == false )
00355 return false;
00356 if( this->readReg( 0x23, &temp_reg2 ) == false )
00357 return false;
00358 oldOffsets[i] = ( (temp_reg1 << 3) | (0x07 & temp_reg2) ) & 0x7F;
00359 }
00360
00361 do
00362 {
00363
00364 uint8_t local_reg;
00365
00366 if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00367 return false;
00368
00369
00370 local_reg &= ~0x03;
00371
00372 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00373 {
00374 ROS_ERROR( "BMA180::Calibrate(): failed." );
00375 return false;
00376 }
00377
00378
00379
00380
00381 local_reg |= (0x01 << offset_finetuning);
00382
00383 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00384 {
00385 ROS_ERROR( "BMA180::Calibrate(): failed." );
00386 return false;
00387 }
00388
00389
00390
00391
00392 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00393 {
00394 ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00395 return false;
00396 }
00397
00398
00399 sleep( 2 );
00400
00401
00402
00403 if( i == 0 )
00404 {
00405 if( this->readReg( 0x29, &newOffsets[i] ) == false )
00406 return false;
00407 newOffsets[i] = (newOffsets[i] >> 1) & 0x7F;
00408 }
00409
00410
00411 if( i == 1 )
00412 {
00413 if( this->readReg( 0x29, &newOffsets[i] ) == false )
00414 return false;
00415 newOffsets[i] = (newOffsets[i] >> 1) & 0x7F;
00416 }
00417
00418
00419 if( i == 2 )
00420 {
00421 uint8_t temp_reg1;
00422 uint8_t temp_reg2;
00423
00424 if( this->readReg( 0x25, &temp_reg1 ) == false )
00425 return false;
00426 if( this->readReg( 0x23, &temp_reg2 ) == false )
00427 return false;
00428
00429 newOffsets[i] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;
00430
00431 }
00432
00433
00434 ROS_INFO( "Old Offset: %x New Offset: %x", oldOffsets[i], newOffsets[i] );
00435 } while( (newOffsets[i] == 0x3f) || (newOffsets[i] == 0x40) );
00436
00437
00438 ROS_INFO( "BMA180::Calibrate(): END Calibrating axis %d.", i );
00439 }
00440 ROS_INFO( "BMA180::Calibrate(): Calibration COMPLETE." );
00441
00442 return true;
00443 }
00444
00445
00446
00447
00448
00449 bool BMA180::CoarseCalibrate()
00450 {
00451 uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00452 int16_t oldOffsets[3];
00453 int16_t newOffsets[3];
00454
00455 uint8_t local_offset_z;
00456 uint8_t local_offset_y;
00457 uint8_t local_offset_x;
00458 uint8_t local_offset_lsb1;
00459 uint8_t local_offset_lsb2;
00460
00461
00462 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00463 return false;
00464 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00465 return false;
00466 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00467 return false;
00468 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00469 return false;
00470 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00471 return false;
00472
00473
00474 oldOffsets[0] = int(( ((int16_t)( (local_offset_x ^= 0x80) << 8)) | ( local_offset_lsb1) ) >> 4);
00475 oldOffsets[1] = int(( ((int16_t)( (local_offset_y ^= 0x80) << 8)) | ( local_offset_lsb2 << 4) ) >> 4);
00476 oldOffsets[2] = int(( ((int16_t)( (local_offset_z ^= 0x80) << 8)) | ( local_offset_lsb2) ) >> 4);
00477
00478
00479 ROS_INFO( "Old Coarse Offsets:" );
00480 ROS_INFO( "X: %f", oldOffsets[0] * this->getSensitivity() );
00481 ROS_INFO( "Y: %f", oldOffsets[1] * this->getSensitivity() );
00482 ROS_INFO( "Z: %f", oldOffsets[2] * this->getSensitivity() );
00483
00484
00485 for( int i = 0; i < 3; i++ )
00486 {
00487 ROS_INFO( "BMA180::Calibrate(): BEGIN Calibrating axis %d", i );
00488
00489
00490 uint8_t local_reg;
00491
00492 if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00493 return false;
00494
00495
00496 local_reg &= ~0x03;
00497
00498 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00499 {
00500 ROS_ERROR( "BMA180::Calibrate(): failed." );
00501 return false;
00502 }
00503
00504
00505
00506 local_reg |= 0x02;
00507
00508 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00509 {
00510 ROS_ERROR( "BMA180::Calibrate(): failed." );
00511 return false;
00512 }
00513
00514
00515
00516 if( i != 2 )
00517 {
00518 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00519 {
00520 ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00521 return false;
00522 }
00523
00524 sleep( 2 );
00525 }
00526
00527
00528 if( i == 2 )
00529 {
00530
00531 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00532 {
00533 ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00534 return false;
00535 }
00536
00537 sleep( 2 );
00538
00539
00540
00541
00542
00543 uint8_t new_offset_z;
00544 uint8_t new_offset_lsb2;
00545
00546 if( this->readReg( ADDRESS_OFFSET_Z, &new_offset_z ) == false )
00547 return false;
00548 if( this->readReg( ADDRESS_OFFSET_LSB2, &new_offset_lsb2 ) == false )
00549 return false;
00550
00551
00552 uint16_t rawTC = ((uint16_t)new_offset_z << 8) |(uint16_t)new_offset_lsb2;
00553 rawTC ^= 0x8000;
00554
00555 int16_t one_g;
00556
00557 switch( accel_range_ )
00558 {
00559 case RANGE_1: {}
00560 case RANGE_1_5: {}
00561 case RANGE_2: {}
00562 case RANGE_4:
00563 one_g = (0x009A << 4);
00564 break;
00565 case RANGE_8: {}
00566 case RANGE_16:
00567 one_g = (0x008F << 4);
00568 break;
00569 default:
00570 one_g = (0x008F << 4);
00571 }
00572
00573 int16_t sum_ob = (int16_t)rawTC + one_g;
00574
00575 sum_ob ^= 0x8000;
00576
00577 new_offset_z = (uint8_t)(sum_ob >> 8);
00578 new_offset_lsb2 = (uint8_t)(0x00FF & sum_ob);
00579
00580 ROS_INFO( "offset_z: %x", new_offset_z );
00581 ROS_INFO( "offset_lsb2: %x", new_offset_lsb2 );
00582
00583
00584 if( this->writeToRegAndVerify( ADDRESS_OFFSET_Z, new_offset_z, new_offset_z ) == false )
00585 return false;
00586 if( this->writeToRegAndVerify( ADDRESS_OFFSET_LSB2, new_offset_lsb2, new_offset_lsb2 ) == false )
00587 return false;
00588 }
00589 ROS_INFO( "BMA180::Calibrate(): END Calibrating axis %d \r\n", i );
00590 }
00591
00592
00593 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00594 return false;
00595 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00596 return false;
00597 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00598 return false;
00599 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00600 return false;
00601 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00602 return false;
00603
00604
00605 newOffsets[0] = int(( ((int16_t)( (local_offset_x ^= 0x80) << 8)) | ( local_offset_lsb1) ) >> 4);
00606 newOffsets[1] = int(( ((int16_t)( (local_offset_y ^= 0x80) << 8)) | ( local_offset_lsb2 << 4) ) >> 4);
00607 newOffsets[2] = int(( ((int16_t)( (local_offset_z ^= 0x80) << 8)) | ( local_offset_lsb2) ) >> 4);
00608
00609 ROS_INFO( "New Coarse Offsets:" );
00610 ROS_INFO( "X: %f", newOffsets[0] * this->getSensitivity() );
00611 ROS_INFO( "Y: %f", newOffsets[1] * this->getSensitivity() );
00612 ROS_INFO( "Z: %f", newOffsets[2] * this->getSensitivity() );
00613
00614 ROS_INFO( "BMA180::Calibrate(): COARSE CALIBRATION COMPLETE." );
00615
00616 return true;
00617 }
00618
00619
00620
00621
00622
00623 bool BMA180::TwoStepCoarseCalibrate()
00624 {
00625 uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00626 std::string axes[3] = { "X", "Y", "Z" };
00627 int16_t oldOffsets[3];
00628 int16_t newOffsets[3];
00629
00630 uint8_t local_offset_z;
00631 uint8_t local_offset_y;
00632 uint8_t local_offset_x;
00633 uint8_t local_offset_lsb1;
00634 uint8_t local_offset_lsb2;
00635
00636
00637 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00638 return false;
00639 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00640 return false;
00641 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00642 return false;
00643 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00644 return false;
00645 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00646 return false;
00647
00648 oldOffsets[0] = ((((local_offset_x << 8) | local_offset_lsb1) >> 4) & 0x0FFF);
00649 oldOffsets[1] = (((local_offset_y << 8) | (local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00650 oldOffsets[2] = ((((local_offset_z << 8) | local_offset_lsb2) >> 4) & 0x0FFF);
00651
00652 for( int i = 0; i < 3; i++ )
00653 {
00654 ROS_INFO( "BEGIN Calibrating axis %s.", axes[i].c_str() );
00655
00656 uint8_t local_reg;
00657
00658 if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00659 return false;
00660
00661
00662 local_reg &= ~0x03;
00663
00664 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00665 {
00666 ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): failed." );
00667 return false;
00668 }
00669
00670
00671 local_reg |= 0x02;
00672
00673 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00674 {
00675 ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): failed." );
00676 return false;
00677 }
00678
00679
00680 if( i != 2 )
00681 {
00682 if( i == 0 )
00683 {
00684
00685
00686 std::cout << std::endl << "Place the sensor on a still, flat surface." << std::endl;
00687 std::cout << "Ready to calibrate x and y axes. Continue? [y/n]" << std::endl;
00688 std::string reply;
00689 std::getline( std::cin, reply );
00690 if( reply != "Y" && reply != "y" )
00691 {
00692 ROS_INFO( "Calibration cancelled by user." );
00693 return true;
00694 }
00695 }
00696
00697
00698 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00699 {
00700 ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): could not set bitFlag to begin calibration." );
00701 return false;
00702 }
00703
00704 sleep( 2 );
00705 }
00706
00707
00708 if( i == 2 )
00709 {
00710
00711 std::cout << std::endl << "Ready to calibrate z axis." << std::endl
00712 << "Orient sensor such that z-axis is perpendicular to gravity." << std::endl
00713 << "Do not move the sensor in this position." <<std::endl
00714 << "Enter [y/n] to continue/cancel calibration." << std::endl;
00715 std::string reply;
00716 std::getline( std::cin, reply );
00717 if( reply != "Y" && reply != "y" )
00718 {
00719 ROS_INFO( "Calibration cancelled by user." );
00720 return false;
00721 }
00722
00723
00724 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00725 {
00726 ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): could not set bitFlag to begin calibration." );
00727 return false;
00728 }
00729
00730 sleep( 2 );
00731 }
00732
00733 ROS_INFO( "BMA180::TwoStepCoarseCalibrate(): END Calibrating axis %d.", i );
00734 }
00735
00736
00737 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00738 return false;
00739 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00740 return false;
00741 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00742 return false;
00743 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00744 return false;
00745 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00746 return false;
00747
00748 newOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00749 newOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00750 newOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00751
00752
00753 std::cout << "(in 12-bit offset-binary format)" << std::endl;
00754 std::cout << "Old Coarse Offsets | New Coarse Offsets " << std::endl;
00755 std::cout << " " << std::hex << oldOffsets[0]
00756 << " " << std::hex << newOffsets[0] << std::endl;
00757 std::cout << " " << std::hex << oldOffsets[1]
00758 << " " << std::hex << newOffsets[1] << std::endl;
00759 std::cout << " " << std::hex << oldOffsets[2]
00760 << " " << std::hex << newOffsets[2] << std::endl;
00761
00762 ROS_INFO( "BMA180::Calibrate(): COARSE CALIBRATION COMPLETE." );
00763
00764 return true;
00765 }
00766
00767
00768
00769
00770
00771 bool BMA180::FullCalibration()
00772 {
00773 uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00774 std::string axes[3] = { "X", "Y", "Z" };
00775
00776 int16_t oldOffsets[3];
00777 int16_t newOffsets[3];
00778
00779 uint8_t oldFineOffsets[3];
00780 uint8_t newFineOffsets[3];
00781
00782 uint8_t local_offset_z;
00783 uint8_t local_offset_y;
00784 uint8_t local_offset_x;
00785 uint8_t local_offset_lsb1;
00786 uint8_t local_offset_lsb2;
00787
00788
00789 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00790 return false;
00791 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00792 return false;
00793 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00794 return false;
00795 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00796 return false;
00797 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00798 return false;
00799
00800 oldOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00801 oldOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00802 oldOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00803
00804
00805 if( this->readReg( 0x29, &oldFineOffsets[0] ) == false )
00806 return false;
00807 oldFineOffsets[0] = (oldFineOffsets[0] >> 1) & 0x7F;
00808
00809 if( this->readReg( 0x26, &oldFineOffsets[1] ) == false )
00810 return false;
00811 oldFineOffsets[1] = (oldFineOffsets[1] >> 1) & 0x7F;
00812
00813 uint8_t temp_reg1, temp_reg2;
00814 if( this->readReg( 0x25, &temp_reg1 ) == false )
00815 return false;
00816 if( this->readReg( 0x23, &temp_reg2 ) == false )
00817 return false;
00818 oldFineOffsets[2] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;
00819
00820 for( int i = 0; i < 3; i++ )
00821 {
00822 ROS_INFO( "BEGIN Calibrating axis %s.", axes[i].c_str() );
00823
00824 do
00825 {
00826
00827 uint8_t local_reg;
00828
00829 if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00830 return false;
00831
00832
00833 local_reg &= ~0x03;
00834
00835 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00836 {
00837 ROS_ERROR( "BMA180::Calibrate(): failed." );
00838 return false;
00839 }
00840
00841
00842 local_reg |= 0x03;
00843
00844 if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00845 {
00846 ROS_ERROR( "BMA180::Calibrate(): failed." );
00847 return false;
00848 }
00849
00850
00851 if( i == 0 )
00852 {
00853
00854
00855 std::cout << std::endl << "Place the sensor on a still, flat surface." << std::endl
00856 << "Ready to calibrate x and y axes. Continue? [y/n]" << std::endl;
00857 std::string reply;
00858 std::getline( std::cin, reply );
00859 if( reply != "Y" && reply != "y" )
00860 {
00861 ROS_INFO( "Full Calibration cancelled by user." );
00862 return false;
00863 }
00864 }
00865
00866
00867 if( i == 2 )
00868 {
00869
00870 std::cout << std::endl << "Ready to calibrate z-axis." << std::endl
00871 << "Orient sensor such that z-axis is perpendicular to gravity." << std::endl
00872 << "Do not move the sensor in this position." << std::endl
00873 << "Enter [y/n] to continue/cancel calibration." << std::endl;
00874 std::string reply;
00875 std::getline( std::cin, reply );
00876 if ( reply != "Y" && reply != "y" )
00877 {
00878 ROS_INFO( "Full Calibration cancelled by user." );
00879 return false;
00880 }
00881 }
00882
00883
00884 if( this->setEnOffsetBit( bitFlags[i] ) == false )
00885 {
00886 ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00887 return false;
00888 }
00889
00890 sleep( 2 );
00891
00892
00893
00894 if( i == 0 )
00895 {
00896 if( this->readReg( 0x29, &newFineOffsets[i] ) == false )
00897 return false;
00898 newFineOffsets[i] = (newFineOffsets[i] >> 1) & 0x7F;
00899 }
00900
00901 if( i == 1 )
00902 {
00903 if( this->readReg( 0x29, &newFineOffsets[i] ) == false )
00904 return false;
00905 newFineOffsets[i] = (newFineOffsets[i] >> 1) & 0x7F;
00906 }
00907
00908 if( i == 2 )
00909 {
00910 uint8_t temp_reg1, temp_reg2;
00911
00912 if( this->readReg( 0x25, &temp_reg1 ) == false )
00913 return false;
00914 if( this->readReg( 0x23, &temp_reg2 ) == false )
00915 return false;
00916 newFineOffsets[i] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;
00917 }
00918
00919
00920
00921 ROS_INFO( "NewFineOffsets: %x", newFineOffsets[i] );
00922
00923 } while( (newFineOffsets[i] == 0x3f) || (newFineOffsets[i] == 0x40) );
00924
00925 ROS_INFO( "BMA180::FullCalibration(): END Calibrating axis %d.", i );
00926 }
00927
00928
00929 if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00930 return false;
00931 if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00932 return false;
00933 if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00934 return false;
00935 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00936 return false;
00937 if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00938 return false;
00939
00940 newOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00941 newOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00942 newOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00943
00944 std::cout << "(in 12-bit offset-binary format)" << std::endl;
00945 std::cout << "Old Coarse Offsets | New Coarse Offsets " << std::endl;
00946 std::cout << "X: " << std::hex << oldOffsets[0]
00947 << " " << std::hex << newOffsets[0] << std::endl;
00948 std::cout << "Y: " << std::hex << oldOffsets[1]
00949 << " " << std::hex << newOffsets[1] << std::endl;
00950 std::cout << "Z: " << std::hex << oldOffsets[2]
00951 << " " << std::hex << newOffsets[2] << std::endl;
00952
00953 std::cout << "(in 7-bit offset-binary format)" << std::endl;
00954 std::cout << "Old Fine Offsets | New Fine Offsets " << std::endl;
00955 std::cout << "X: " << std::hex << (int)oldFineOffsets[0]
00956 << " " << std::hex << (int)newFineOffsets[0] << std::endl;
00957 std::cout << "Y: " << std::hex << (int)oldFineOffsets[1]
00958 << " " << std::hex << (int)newFineOffsets[1] << std::endl;
00959 std::cout << "Z: " << std::hex << (int)oldFineOffsets[2]
00960 << " " << std::hex << (int)newFineOffsets[2] << std::endl;
00961
00962 ROS_INFO( "BMA180::FullCalibration(): FULL CALIBRATION COMPLETE." );
00963
00964 return true;
00965 }
00966
00967
00968
00969
00970
00971 bool BMA180::DisableI2C()
00972 {
00973
00974 uint8_t ctrl_register = (uint8_t)CMD_DISABLE_I2C;
00975
00976 switch( this->getProtocol() )
00977 {
00978 case SPI:
00979 if( this->writeToReg( ADDRESS_HIGH_DUR, ctrl_register ) == false )
00980 {
00981 ROS_ERROR( "BMA180::DisableI2C(): write failed." );
00982 return false;
00983 }
00984 return true;
00985 default:
00986 ROS_ERROR( "BMA180::DisableI2C() cannot disable i2c from this protocol." );
00987 }
00988 return false;
00989 }
00990
00991
00992
00993
00994
00995 bool BMA180::EnableWriting()
00996 {
00997 uint8_t reg_value;
00998
00999
01000 if( this->readReg( ADDRESS_CTRLREG0, ®_value ) == false )
01001 {
01002 ROS_ERROR( "BMA180::EnableWriting(): read failed." );
01003 return false;
01004 }
01005
01006
01007 ROS_INFO( "EnableWriting flag before: %d. Default: 0", (reg_value & (1 << ee_w) ) >> ee_w );
01008
01009
01010
01011 reg_value |= (1 << ee_w);
01012
01013
01014 if( this->writeToReg( ADDRESS_CTRLREG0, reg_value ) == false )
01015 {
01016 ROS_ERROR( "BMA180::EnableWriting(): write failed." );
01017 return false;
01018 }
01019
01020
01021 if( this->readReg( ADDRESS_CTRLREG0, ®_value ) == false )
01022 {
01023 ROS_ERROR( "BMA180::EnableWriting(): read failed." );
01024 return false;
01025 }
01026
01027
01028 uint8_t enable_actual = ( reg_value & (1 << ee_w) ) >> ee_w;
01029 uint8_t enable_expected = 1;
01030
01031
01032 ROS_INFO("EnableWriting flag after: %d. Expected: %d", enable_actual, enable_expected );
01033
01034
01035 if( enable_expected != enable_actual )
01036 {
01037 ROS_ERROR( "BMA180::EnableWriting(): failed." );
01038 return false;
01039 }
01040
01041
01042 return true;
01043 }
01044
01045
01046
01047
01048
01049 bool BMA180::changeAccelRange()
01050 {
01051 uint8_t local_range;
01052
01053
01054 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_range ) == false )
01055 {
01056 ROS_ERROR("BMA180::changeAccelRange(): read failed.");
01057 return false;
01058 }
01059
01060
01061 ROS_INFO( "AccelRange bits before: %d. Default: %d", ( (local_range & (0x07 << range)) >> range), 2);
01062
01063
01064
01065 local_range &= ~(0x07 << range);
01066 local_range |= (accel_range_ << range);
01067
01068
01069 if( this->writeToReg( ADDRESS_OFFSET_LSB1, local_range ) == false )
01070 {
01071 ROS_ERROR( "BMA180::changeAccelRange(): write failed." );
01072 return false;
01073 }
01074
01075
01076 if( this->readReg( ADDRESS_OFFSET_LSB1, &local_range ) == false )
01077 {
01078 ROS_ERROR( "BMA180::changeAccelRange(): read failed." );
01079 return false;
01080 }
01081
01082
01083 uint8_t range_actual = ( local_range & (0x07 << range) ) >> range;
01084 uint8_t range_expected = (uint8_t) accel_range_;
01085
01086
01087 ROS_INFO( "AccelRange bits after: %d. Expected: %d", range_actual, range_expected );
01088
01089
01090 if( range_expected != range_actual )
01091 {
01092 ROS_ERROR( "BMA180::changeAccelRange(): failed." );
01093 return false;
01094 }
01095
01096
01097 return true;
01098 }
01099
01100
01101
01102
01103 bool BMA180::changeBandwidth()
01104 {
01105 uint8_t local_bw_reg;
01106
01107 if( this->readReg( ADDRESS_BW_TCS, &local_bw_reg ) == false )
01108 {
01109 ROS_ERROR( "BMA180::changeBandwidth(): read failed." );
01110 return false;
01111 }
01112
01113
01114 ROS_INFO( "Bandwidth bits before: %d. Default: %d", ( (local_bw_reg & (0x0F << bw)) >> bw), 4 );
01115
01116
01117
01118 local_bw_reg &= ~(0x0F << bw);
01119 local_bw_reg |= bandwidth_ << bw;
01120
01121
01122 if( this->writeToReg( ADDRESS_BW_TCS, local_bw_reg ) == false )
01123 {
01124 ROS_ERROR( "BMA180::changeBandwidth(): write failed." );
01125 return false;
01126 }
01127
01128
01129 if( this->readReg( ADDRESS_BW_TCS, &local_bw_reg ) == false )
01130 {
01131 ROS_ERROR( "BMA180::changeBandwidth(): read failed." );
01132 return false;
01133 }
01134
01135
01136 uint8_t bandwidth_actual = ( local_bw_reg & (0x0F << bw) ) >> bw;
01137 uint8_t bandwidth_expected = (uint8_t) bandwidth_;
01138
01139
01140 ROS_INFO("Bandwidth bits after: %d. Expected: %d", bandwidth_actual, bandwidth_expected);
01141
01142
01143 if( bandwidth_expected != bandwidth_actual )
01144 {
01145 ROS_ERROR( "BMA180::changeBandwidth(): failed." );
01146 return false;
01147 }
01148
01149
01150 return true;
01151 }
01152
01153
01154
01155
01156 bool BMA180::setEnOffsetBit( uint8_t bit )
01157 {
01158 uint8_t local_ctrl_reg1;
01159
01160
01161
01162 if( this->readReg( ADDRESS_CTRLREG1, &local_ctrl_reg1 ) == false )
01163 {
01164 ROS_ERROR( "BMA180::setEnOffsetBit(): read failed." );
01165 ROS_INFO("en_offset bit before: %d. Default: 0", ((local_ctrl_reg1 >> bit) & 0x01) );
01166 return false;
01167 }
01168
01169
01170
01171
01172 local_ctrl_reg1 &= ~(1 << bit);
01173 local_ctrl_reg1 |= (1 << bit);
01174
01175
01176 if( this->writeToReg( ADDRESS_CTRLREG1, local_ctrl_reg1 ) == false )
01177 {
01178 ROS_ERROR( "BMA180::setEnOffsetBit(): write failed." );
01179 return false;
01180 }
01181
01182
01183 if( this->readReg( ADDRESS_CTRLREG1, &local_ctrl_reg1 ) == false )
01184 {
01185 ROS_ERROR( "BMA180::setEnOffsetBit(): read failed." );
01186 return false;
01187 }
01188
01189
01190 uint8_t actual = ( local_ctrl_reg1 >> bit ) & 0x01;
01191 uint8_t expected = 1;
01192
01193
01194
01195
01196
01197 if( expected != actual )
01198 {
01199 ROS_ERROR( "BMA180::setEnOffsetBit(): failed." );
01200 ROS_INFO( "en_offset bit after: %d. Expected: %d", actual, expected );
01201 return false;
01202 }
01203
01204 return true;
01205 }
01206
01207
01208
01209
01210 bool BMA180::readReg( uint8_t reg, uint8_t* value )
01211 {
01212
01213 switch( this->getProtocol() )
01214 {
01215 case I2C:
01216 if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 )
01217 {
01218 ROS_ERROR( "BMA180::readReg(): Error reading register via I2C!" );
01219 return false;
01220 }
01221 break;
01222 case SPI:
01223
01224 if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), ( 1 << SPI_READ_FLAG ) | reg, value, 1 ) < 0 )
01225 {
01226 ROS_ERROR( "BMA180::readReg(): Error reading register via SPI!" );
01227 return false;
01228 }
01229 break;
01230 default:
01231
01232 ROS_ERROR( "BMA180::readReg(...): invalid protocol." );
01233 return false;
01234 }
01235 return true;
01236 }
01237
01238
01239
01240
01241 bool BMA180::writeToReg( uint8_t reg, uint8_t value )
01242 {
01243
01244 switch( this->getProtocol() )
01245 {
01246 case I2C:
01247 if( hardware_->write( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0 )
01248 {
01249 ROS_ERROR( "BMA180::writeToReg(): Error writing to register via I2C!" );
01250 return false;
01251 }
01252 break;
01253 case SPI:
01254
01255 if( hardware_->write( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)®), (uint8_t*)&value, 1) < 0 )
01256 {
01257 ROS_ERROR( "BMA180::writeToReg(): Error writing to register via SPI!" );
01258 return false;
01259 }
01260 break;
01261 default:
01262 ROS_ERROR("BMA180::writeToReg(...): invalid protocol.");
01263 return false;
01264 }
01265 return true;
01266 }
01267
01268
01269
01270
01271
01272 bool BMA180::writeToRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01273 {
01274 uint8_t actual;
01275
01276 if( this->writeToReg( reg, value ) == false )
01277 return false;
01278
01279 if( this->readReg( reg, &actual ) == false )
01280 return false;
01281
01282 if( expected != actual )
01283 {
01284 ROS_ERROR( "BMA180::writeToRegAndVerify(...): failed." );
01285 ROS_ERROR( "(in Hex) expected: %x actual: %x", expected, actual );
01286 return false;
01287 }
01288
01289 return true;
01290 }
01291
01292
01293 bool BMA180::readSensorData( uint8_t reg, uint8_t* data, uint8_t num_bytes )
01294 {
01295
01296 switch( this->getProtocol() )
01297 {
01298 case I2C:
01299 if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, data, num_bytes ) < 0 )
01300 {
01301 ROS_ERROR( "BMA180::readSensorData(): Error reading register via I2C!" );
01302 return false;
01303 }
01304 break;
01305 case SPI:
01306
01307 if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), ((1 << SPI_READ_FLAG)|reg), data, num_bytes ) < 0 )
01308 {
01309 ROS_ERROR( "BMA180::readSensorData(): Error reading register via SPI!" );
01310 return false;
01311 }
01312 break;
01313 default:
01314
01315 ROS_ERROR( "BMA180::readSensorData(...): invalid protocol." );
01316 return false;
01317 }
01318 return true;
01319 }