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 #include "bmc050_driver/bmc050.hpp"
00051 #include "bmc050_driver/bmc050_parameters.hpp"
00052
00053
00054
00055 BMC050::BMC050( bosch_hardware_interface* hw ) :
00056 sensor_driver( hw ),
00057 BMC050Parameters()
00058 {
00059
00060
00061 SensitivityXY_ = 0.3;
00062 SensitivityZ_ = 0.3;
00063 TempSlope_ = 0.5;
00064
00065 prev_bw_reg_ = BW_1000HZ;
00066 }
00067
00068
00069 BMC050::~BMC050()
00070 {
00071 }
00072
00073
00074
00075
00076
00077 bool BMC050::initialize()
00078 {
00079
00080 if( hardware_->initialize() == false )
00081 {
00082 ROS_ERROR("bmc050::initialize(): Could not initialize a hardware interface!");
00083 return false;
00084 }
00085
00086 this->softResetCompass();
00087 this->softResetAccel();
00088
00089
00090
00091 usleep( 3000 );
00092
00093
00094
00095 if( this->readRegistersAccel() == false )
00096 return false;
00097 if( this->readRegistersCompass() == false )
00098 return false;
00099
00100
00101 if( this->enterNormalModeAccel() == false )
00102 return false;
00103 if( this->enterNormalModeCompass() == false )
00104 return false;
00105 this->readRegistersAccel();
00106
00107 this->readRegistersCompass();
00108
00109
00110
00111
00112 ROS_INFO("bmc050::initialize(): adjusting Accelerometer range.");
00113 if( this->changeAccelRange() == false )
00114 return false;
00115 ROS_INFO("bmc050::initialize(): Accelerometer range adjusted.");
00116
00117 ROS_INFO("bmc050::initialize(): adjusting Compass output data rate.");
00118 if( this->changeCompassRate() == false )
00119 return false;
00120 ROS_INFO("bmc050::initialize(): Compass rate adjusted.");
00121
00122 ROS_INFO("bmc050::initialize(): Applying Filter parameter.");
00123 if( this->filterData(accel_is_filtered_) == false )
00124 return false;
00125 ROS_INFO("bmc050::initialize(): Filter parameter applied.");
00126
00127 ROS_INFO("bmc050::initialize(): Adjusting Accelerometer Bandwidth.");
00128 if( this->changeAccelBandwidth() == false )
00129 return false;
00130 ROS_INFO("bmc050::initialize(): Bandwidth adjusted.");
00131
00132 ROS_INFO("bmc050::initialize(): Adjusting Compass X,Y,Z number of samples.");
00133 if( this->changeNumRepetitionsXY() == false )
00134 return false;
00135 if( this->changeNumRepetitionsZ() == false )
00136 return false;
00137 if( this->readRegistersAccel() == false )
00138 return false;
00139 if( this->readRegistersCompass() == false )
00140 return false;
00141
00142 return true;
00143 }
00144
00145
00146
00147
00148
00149
00150 bool BMC050::softResetAccel()
00151 {
00152
00153 if( this->writeToAccelReg( SOFTRESET_ACCEL, SOFTRESET_ACCEL_CMD ) == false )
00154 {
00155 ROS_ERROR("bmc050::softResetAccel(): failed.");
00156 return false;
00157 }
00158
00159 usleep( 2000 );
00160 accel_mode_ = accel_normal_;
00161
00162
00163 prev_bw_reg_ = BW_1000HZ;
00164
00165 return true;
00166 }
00167
00168
00169 bool BMC050::softResetCompass()
00170 {
00171 if( this->writeToAccelReg( SOFTRESET_COMPASS_REG, SOFTRESET_COMPASS_CMD ) == false )
00172 {
00173 ROS_ERROR("bmc050::softResetCompass(): failed.");
00174 return false;
00175 }
00176
00177 usleep( 1000 );
00178
00179
00180 compass_mode_ = compass_suspend_;
00181
00182 return true;
00183 }
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 bool BMC050::changeCompassRate()
00194 {
00195 uint8_t local_rate;
00196
00197
00198 if( this->readRegCompass(RATE_COMPASS, &local_rate ) == false )
00199 return false;
00200
00201
00202 ROS_INFO("Compass rate bits before: %d. Default: %d", ((local_rate >> Data_Rate) & 0x07), 0);
00203
00204
00205
00206 local_rate &= 0xC7;
00207
00208
00209 switch( compass_rate_ )
00210 {
00211 case ODR_10HZ:
00212 local_rate |= (ODR_10HZ << Data_Rate);
00213 break;
00214 case ODR_2HZ:
00215 local_rate |= (ODR_2HZ << Data_Rate);
00216 break;
00217 case ODR_6HZ:
00218 local_rate |= (ODR_6HZ << Data_Rate);
00219 break;
00220 case ODR_8HZ:
00221 local_rate |= (ODR_8HZ << Data_Rate);
00222 break;
00223 case ODR_15HZ:
00224 local_rate |= (ODR_15HZ << Data_Rate);
00225 break;
00226 case ODR_20HZ:
00227 local_rate |= (ODR_20HZ << Data_Rate);
00228 break;
00229 case ODR_25HZ:
00230 local_rate |= (ODR_25HZ << Data_Rate);
00231 break;
00232 case ODR_30HZ:
00233 local_rate |= (ODR_30HZ << Data_Rate);
00234 break;
00235 default:
00236
00237 ROS_ERROR("bmc050::changeCompassRate(): invalid compass rate.");
00238 return false;
00239 }
00240
00241
00242
00243 if( this->writeToCompassReg(RATE_COMPASS, local_rate ) == false )
00244 return false;
00245
00246
00247 if( this->readRegCompass(RATE_COMPASS, &local_rate ) == false )
00248 return false;
00249
00250
00251 uint8_t expected_reg = compass_rate_ ;
00252 uint8_t actual_reg = (local_rate >> Data_Rate) & 0x07;
00253
00254 if( expected_reg != actual_reg )
00255 {
00256 ROS_ERROR("Compass rate bits after: %d. Expected: %d", actual_reg, expected_reg);
00257 return false;
00258 }
00259 else
00260 {
00261
00262 ROS_INFO("Compass rate bits after: %d. Expected: %d", actual_reg, expected_reg);
00263
00264 }
00265
00266 return true;
00267 }
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 bool BMC050::readRegistersAccel()
00280 {
00281
00282 uint8_t mode_change_accel_reg;
00283 uint8_t set_filter_reg;
00284
00285 switch( protocol_ )
00286 {
00287 case I2C:
00288
00289 mode_change_accel_reg = MODECHANGE_ACCEL;
00290 set_filter_reg = SET_FILTER;
00291 break;
00292 case SPI:
00293
00294 mode_change_accel_reg = FLAG_R | MODECHANGE_ACCEL;
00295 set_filter_reg = FLAG_R | SET_FILTER;
00296 break;
00297 default:
00298 ROS_ERROR("bmc050::readRegistersAccel(): invalid protocol.");
00299 return false;
00300 }
00301
00302 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), mode_change_accel_reg, &modechange_accel_, 1 ) < 0 )
00303 {
00304 ROS_ERROR("bmc050::readRegistersAccel(): MODECHANGE_ACCEL read failed.");
00305 return false;
00306 }
00307
00308 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), set_filter_reg, &set_filter_, 1 ) < 0 )
00309 {
00310 ROS_ERROR("bmc050::readRegistersAccel(): SET_FILTER read failed.");
00311 return false;
00312 }
00313
00314
00315
00316
00317 return true;
00318 }
00319
00320
00321
00322
00323
00324 bool BMC050::readRegistersCompass()
00325 {
00326
00327
00328
00329
00330
00331 uint8_t mode_reg;
00332 uint8_t reset_reg;
00333
00334 switch( protocol_ )
00335 {
00336 case I2C:
00337
00338 mode_reg = RATE_COMPASS;
00339 reset_reg = SOFTRESET_COMPASS_REG;
00340 break;
00341 case SPI:
00342
00343 mode_reg = FLAG_R | RATE_COMPASS;
00344 reset_reg = FLAG_R | SOFTRESET_COMPASS_REG;
00345 break;
00346 default:
00347 ROS_ERROR("bmc050::readRegistersCompass(): invalid protocol.");
00348 return false;
00349 }
00350
00351
00352 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), mode_reg, &rate_compass_, 1 ) < 0 )
00353 {
00354 ROS_ERROR("bmc050::readRegistersCompass(): RATE_COMPASS read failed.");
00355 return false;
00356 }
00357
00358
00359 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), reset_reg, &softreset_, 1 ) < 0 )
00360 {
00361 ROS_ERROR("bmc050::readRegistersCompass(): SOFTRESET read failed.");
00362 return false;
00363 }
00364
00365
00366 return true;
00367 }
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380 bool BMC050::changeAccelRange()
00381 {
00382
00383 switch( this->getAccelRange() )
00384 {
00385 case RANGE_2:
00386 accel_sensitivity_ = 0.00391;
00387 break;
00388 case RANGE_4:
00389 accel_sensitivity_ = 0.00781;
00390 break;
00391 case RANGE_8:
00392 accel_sensitivity_ = 0.01562;
00393 break;
00394 case RANGE_16:
00395 accel_sensitivity_ = 0.03125;
00396 break;
00397 default:
00398 ROS_ERROR("BMC050::changeAccelRange(): invalid accel range.");
00399 return false;
00400 }
00401
00402 uint8_t local_range;
00403
00404 if( this->readRegAccel( ADDRESS_ACCEL_RANGE, &local_range ) == false )
00405 return false;
00406
00407 ROS_INFO("AccelRange bits before: %d. Default: %d", (local_range & (0x0F)), 3);
00408
00409
00410 local_range &= ~( 0x0F );
00411 local_range |= accel_range_;
00412
00413
00414 if( this->writeToAccelReg( ADDRESS_ACCEL_RANGE, local_range ) == false )
00415 return false;
00416
00417
00418 if( this->readRegAccel( ADDRESS_ACCEL_RANGE, &local_range ) == false)
00419 return false;
00420
00421
00422 uint8_t expected_reg = accel_range_;
00423 uint8_t actual_reg = local_range;
00424
00425
00426 ROS_INFO("AccelRange bits after: %d. Expected: %d", actual_reg, expected_reg);
00427
00428
00429 if( expected_reg != actual_reg )
00430 return false;
00431
00432 return true;
00433 }
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447 bool BMC050::filterData( bool request )
00448 {
00449 uint8_t local_bandwidth;
00450
00451 if( this->readRegAccel( SET_FILTER, &local_bandwidth ) == false )
00452 return false;
00453
00454
00455 ROS_INFO("Accelerometer bandwidth bits before: %d. Default: %d", ((local_bandwidth >> DATA_HIGH_BW) & 0x01), 0);
00456
00457
00458
00459 if( request == true )
00460 local_bandwidth &= ~( 1 << DATA_HIGH_BW );
00461 else
00462 local_bandwidth |= ( 1 << DATA_HIGH_BW );
00463
00464
00465 if( this->writeToAccelReg( SET_FILTER, local_bandwidth ) == false )
00466 return false;
00467
00468
00469 if( this->readRegAccel( SET_FILTER, &local_bandwidth ) == false )
00470 return false;
00471
00472
00473 uint8_t expected_reg = !accel_is_filtered_;
00474 uint8_t actual_reg = ( (local_bandwidth >> DATA_HIGH_BW) & 0x01 );
00475
00476
00477 ROS_INFO("Accelerometer bandwidth bits after: %d. Expected: %d", actual_reg, expected_reg);
00478
00479
00480 if( expected_reg != actual_reg )
00481 return false;
00482
00483 return true;
00484 }
00485
00486
00487
00488
00489 bool BMC050::changeAccelBandwidth()
00490 {
00491 uint8_t local_bw_reg;
00492
00493 if( this->readRegAccel( 0x10, &local_bw_reg ) == false )
00494 return false;
00495
00496 ROS_INFO("Accelerometer bandwidth setting before: %d. Default: %d", (local_bw_reg & 0x1F), 0x1F);
00497
00498
00499 local_bw_reg &= ~( 0x1F );
00500 local_bw_reg |= bw_reg_;
00501
00502 if( this->writeToAccelReg( 0x10, local_bw_reg ) == false )
00503 return false;
00504
00505 uint8_t actual_reg;
00506 if( this->readRegAccel( 0x10, &actual_reg) == false )
00507 return false;
00508
00509 uint8_t actual_bw = actual_reg & 0x1F;
00510 uint8_t expected_bw = bw_reg_;
00511
00512 if( expected_bw != actual_bw )
00513 {
00514 ROS_ERROR("Accelerometer bandwidth setting after: %d. Expected: %d", actual_bw, bw_reg_);
00515 ROS_ERROR("BMC050::changeAccelBandwidth(): failed.");
00516 return false;
00517 }
00518 else
00519 ROS_INFO("Accelerometer bandwidth setting after: %d. Expected: %d", actual_bw, bw_reg_);
00520 return true;
00521 }
00522
00523
00524
00525
00526 bool BMC050::changeNumRepetitionsXY()
00527 {
00528 uint8_t reg_before;
00529
00530 this->readRegCompass( ADDRESS_COMPASS_REPXY, ®_before );
00531 ROS_INFO("Compass REPXY before: %d Default: %d ", reg_before, 0x00 );
00532
00533
00534 this->writeToCompassReg( ADDRESS_COMPASS_REPXY, repsXY_ );
00535
00536 uint8_t actual;
00537
00538 this->readRegCompass( ADDRESS_COMPASS_REPXY, &actual );
00539
00540 if( repsXY_ != actual )
00541 {
00542 ROS_ERROR("Compass REPXY after: %d Expected: %d ", actual, repsXY_);
00543 return false;
00544 }
00545
00546 ROS_INFO("Compass REPXY after: %d Expected: %d ", actual, repsXY_);
00547 return true;
00548 }
00549
00550
00551
00552
00553 bool BMC050::changeNumRepetitionsZ()
00554 {
00555 uint8_t reg_before;
00556
00557 this->readRegCompass( ADDRESS_COMPASS_REPZ, ®_before );
00558 ROS_INFO( "Compass REPZ before: %d Default: %d", reg_before, 0x00 );
00559
00560
00561 this->writeToCompassReg( ADDRESS_COMPASS_REPZ, repsZ_ );
00562
00563 uint8_t actual;
00564
00565 this->readRegCompass( ADDRESS_COMPASS_REPZ, &actual );
00566
00567 if( repsZ_ != actual )
00568 {
00569 ROS_ERROR("Compass REPZ after: %d Expected: %d ", actual, repsZ_);
00570 return false;
00571 }
00572
00573 ROS_INFO("Compass REPZ after: %d Expected: %d", actual, repsZ_);
00574 return true;
00575 }
00576
00577
00578
00579
00580 void BMC050::simpleCalibrationAccel()
00581 {
00582 std::string axes[3] = { "X", "Y", "Z" };
00583 uint8_t offset_target_val[3] = { 0, 0, 1 };
00584 uint8_t offset_target[3] = { offset_target_x, offset_target_y, offset_target_z };
00585 uint8_t axis_bit[3] = { hp_x_en, hp_y_en, hp_z_en };
00586 uint8_t local_offset_reg;
00587 uint8_t local_calibration_status;
00588
00589
00590
00591
00592 this->printOffsets();
00593
00594
00595 std::cout << "Place the sensor flat and upright on a level surface." << std::endl
00596 << "Warning: Do not move the sensor during calibration." << std::endl
00597 <<"Press [y/n] to continue/cancel." << std::endl;
00598 std::string reply;
00599 std::getline( std::cin, reply );
00600 if ((reply != "Y") && (reply != "y"))
00601 {
00602 ROS_INFO("Calibration cancelled by user.");
00603 return;
00604 }
00605
00606
00607 std::cout << "Beginning Simple Calibration routine:" << std::endl << std::endl;
00608 for (int i=0;i<3;i++)
00609 {
00610 std::cout << "Beginning Axis: " << axes[i] << std::endl;
00611
00612 if( this->readRegAccel( ADDRESS_OFFSET_TARGET, &local_offset_reg ) == false )
00613 {
00614 std::cout<< "Error occured" << std::endl;
00615 return;
00616 }
00617 local_offset_reg &= (0x03 << offset_target[i]);
00618 local_offset_reg |= (offset_target_val[i] << offset_target[i]);
00619
00620 if( this->writeToAccelReg( ADDRESS_OFFSET_TARGET, local_offset_reg ) == false )
00621 {
00622 std::cout<< "Error occured" << std::endl;
00623 return;
00624 }
00625
00626
00627 if( this->readRegAccel( ADDRESS_ENABLE_CALIBRATION, &local_calibration_status ) == false )
00628 {
00629 std::cout<< "Error occured" << std::endl;
00630 return;
00631 }
00632 local_calibration_status &= ~0x07;
00633 local_calibration_status |= (1 << axis_bit[i]);
00634
00635 if( this->writeToAccelReg( ADDRESS_ENABLE_CALIBRATION, local_calibration_status ) == false )
00636 {
00637 std::cout<< "Error occured" << std::endl;
00638 return;
00639 }
00640 std::cout << "Calibrating..." << std::endl;
00641
00642 sleep( 3 );
00643 std::cout << "Done Calibrating Axis: " << axes[i] << std::endl << std::endl;
00644
00645 }
00646 std::cout << "ACCELEROMETER CALIBRATION COMPLETE" << std::endl;
00647
00648
00649
00650 this->printOffsets();
00651 }
00652
00653
00654
00655
00656
00657 bool BMC050::takeMeasurement()
00658 {
00659
00660
00661
00662 this->getAccelData();
00663 this->getCompassData();
00664 return true;
00665 }
00666
00667
00668
00669
00670
00671 bool BMC050::getAccelData()
00672 {
00673
00674 uint8_t Data[7];
00675 uint8_t address;
00676
00677 switch( this->getProtocol() )
00678 {
00679 case I2C:
00680 address = ADDRESS_ACCLXYZ;
00681 break;
00682 case SPI:
00683 address = (FLAG_R | ADDRESS_ACCLXYZ);
00684 break;
00685 default:
00686 ROS_ERROR("bmc050::getAccelData(): cannot read from this protocol.");
00687 return false;
00688 }
00689
00690 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 7 ) < 0 )
00691 {
00692 ROS_ERROR("bmc050::getAccelData(): Error reading from hardware interface!");
00693 return false;
00694 }
00695
00696 AccelX_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00697 AccelY_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 6 );
00698 AccelZ_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 6 );
00699
00700 Temperature_ = ( TempSlope_ *(int8_t)Data[6] ) + 24;
00701
00702 return true;
00703 }
00704
00705
00706 double BMC050::getAccelX()
00707 {
00708
00709 uint8_t Data[2];
00710 uint8_t address;
00711
00712 switch( this->getProtocol() )
00713 {
00714 case I2C:
00715 address = ADDRESS_ACCLXYZ;
00716 break;
00717 case SPI:
00718 address = (FLAG_R | ADDRESS_ACCLXYZ);
00719 break;
00720 default:
00721 ROS_ERROR("BMC050::getAccelX(): cannot read from this protocol.");
00722 return false;
00723 }
00724
00725 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00726 {
00727 ROS_ERROR("BMC050::getAccelX(): Error reading from I2C interface!");
00728 return false;
00729 }
00730
00731 AccelX_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00732 return AccelX_;
00733 }
00734
00735
00736
00737
00738
00739 double BMC050::getAccelY()
00740 {
00741
00742 uint8_t Data[2];
00743 uint8_t address;
00744
00745 switch( this->getProtocol() )
00746 {
00747 case I2C:
00748 address = ADDRESS_ACC_Y_LSB;
00749 break;
00750 case SPI:
00751 address = (FLAG_R | ADDRESS_ACC_Y_LSB);
00752 break;
00753 default:
00754 ROS_ERROR("BMC050::getAccelY(): cannot read from this protocol.");
00755 return false;
00756 }
00757
00758 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00759 {
00760 ROS_ERROR("BMC050::getAccelY(): Error reading from I2C interface!");
00761 return false;
00762 }
00763
00764 AccelY_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00765 return AccelY_;
00766 }
00767
00768
00769
00770
00771
00772 double BMC050::getAccelZ()
00773 {
00774
00775 uint8_t Data[2];
00776 uint8_t address;
00777
00778 switch( this->getProtocol() )
00779 {
00780 case I2C:
00781 address = ADDRESS_ACC_Z_LSB;
00782 break;
00783 case SPI:
00784 address = (FLAG_R | ADDRESS_ACC_Z_LSB);
00785 break;
00786 default:
00787 ROS_ERROR("BMC050::getAccelZ(): cannot read from this protocol.");
00788 return false;
00789 }
00790
00791 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00792 {
00793 ROS_ERROR("BMC050::getAccelZ(): Error reading from I2C interface!");
00794 return false;
00795 }
00796
00797 AccelZ_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00798 return AccelZ_;
00799 }
00800
00801
00802
00803
00804
00805 double BMC050::getTemperature()
00806 {
00807
00808 uint8_t Data[2];
00809 uint8_t address;
00810
00811 switch( this->getProtocol() )
00812 {
00813 case I2C:
00814 address = ADDRESS_TEMPERATURE;
00815 break;
00816 case SPI:
00817 address = (FLAG_R | ADDRESS_TEMPERATURE);
00818 break;
00819 default:
00820 ROS_ERROR("BMC050::getTemperature(): cannot read from this protocol.");
00821 return false;
00822 }
00823
00824 if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00825 {
00826 ROS_ERROR("BMC050::getTemperature(): Error reading from I2C interface!");
00827 return false;
00828 }
00829
00830 Temperature_ = ( TempSlope_ *(int8_t)Data[0] ) + 24;
00831 return Temperature_;
00832 }
00833
00834
00835
00836
00837
00838 bool BMC050::getCompassData()
00839 {
00840
00841 uint8_t Data[8];
00842 uint8_t address;
00843
00844 switch( this->getProtocol() )
00845 {
00846 case I2C:
00847 address = ADDRESS_COMPASS_XYZ;
00848 break;
00849 case SPI:
00850 address = (FLAG_R | ADDRESS_COMPASS_XYZ);
00851 break;
00852 default:
00853 ROS_ERROR("BMC050::getCompassData(): cannot read from this protocol.");
00854 return false;
00855 }
00856
00857 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 8 ) < 0 )
00858 {
00859 ROS_ERROR("BMC050::getCompassData(): Error reading from hardware interface!");
00860 return false;
00861 }
00862
00863 CompassX_ = SensitivityXY_ * ( (int16_t)( (int16_t)Data[0] | ((int16_t)Data[1]) << 8 ) / 8);
00864 CompassY_ = SensitivityXY_ * ( (int16_t)( (int16_t)Data[2] | ((int16_t)Data[3]) << 8 ) / 8);
00865 CompassZ_ = SensitivityZ_ * ( (int16_t)( (int16_t)Data[4] | ((int16_t)Data[5]) << 8 ) / 2);
00866 RHall_ = (uint16_t)((uint16_t)Data[6] | ((uint16_t)Data[7] << 8) >> 2 );
00867 return true;
00868 }
00869
00870
00871
00872
00873
00874 double BMC050::getCompassX()
00875 {
00876
00877 uint8_t Data[2];
00878 uint8_t address;
00879
00880
00881 switch( this->getProtocol() )
00882 {
00883 case I2C:
00884 address = ADDRESS_COMPASS_X_LSB;
00885 break;
00886 case SPI:
00887 address = (FLAG_R | ADDRESS_COMPASS_X_LSB);
00888 break;
00889 default:
00890 ROS_ERROR("BMC050::getCompassX(): cannot read from this protocol.");
00891 return false;
00892 }
00893 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00894 {
00895 ROS_ERROR("BMC050::getCompassX(): Error reading from hardware interface!");
00896 return false;
00897 }
00898
00899 CompassX_ = SensitivityXY_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 8);
00900 return CompassX_;
00901 }
00902
00903
00904
00905
00906
00907
00908 double BMC050::getCompassY()
00909 {
00910
00911 uint8_t Data[2];
00912 uint8_t address;
00913
00914
00915 switch( this->getProtocol() )
00916 {
00917 case I2C:
00918 address = ADDRESS_COMPASS_Y_LSB;
00919 break;
00920 case SPI:
00921 address = (FLAG_R | ADDRESS_COMPASS_Y_LSB);
00922 break;
00923 default:
00924 ROS_ERROR("BMC050::getCompassY(): cannot read from this protocol.");
00925 return false;
00926 }
00927 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00928 {
00929 ROS_ERROR("BMC050::getCompassY(): Error reading from hardware interface!");
00930 return false;
00931 }
00932
00933 CompassY_ = SensitivityXY_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 8);
00934 return CompassY_;
00935 }
00936
00937
00938
00939
00940
00941
00942 double BMC050::getCompassZ()
00943 {
00944
00945 uint8_t Data[2];
00946 uint8_t address;
00947
00948
00949 switch( this->getProtocol() )
00950 {
00951 case I2C:
00952 address = ADDRESS_COMPASS_Z_LSB;
00953 break;
00954 case SPI:
00955 address = (FLAG_R | ADDRESS_COMPASS_Z_LSB);
00956 break;
00957 default:
00958 ROS_ERROR("BMC050::getCompassZ(): cannot read from this protocol.");
00959 return false;
00960 }
00961 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00962 {
00963 ROS_ERROR("BMC050::getCompassZ(): Error reading from hardware interface!");
00964 return false;
00965 }
00966
00967 CompassZ_ = SensitivityZ_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 2);
00968 return CompassZ_;
00969 }
00970
00971
00972
00973
00974
00975 uint16_t BMC050::getRHall()
00976 {
00977
00978 uint8_t Data[2];
00979 uint8_t address;
00980
00981
00982 switch( this->getProtocol() )
00983 {
00984 case I2C:
00985 address = ADDRESS_RHALL;
00986 break;
00987 case SPI:
00988 address = (FLAG_R | ADDRESS_RHALL);
00989 break;
00990 default:
00991 ROS_ERROR("BMC050::getRHall(): cannot read from this protocol.");
00992 return false;
00993 }
00994 if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00995 {
00996 ROS_ERROR("BMC050::getRHall(): Error reading from hardware interface!");
00997 return false;
00998 }
00999
01000 RHall_ = (uint16_t)((uint16_t)Data[0] | ((uint16_t)Data[1] << 8) >> 2 );
01001 return RHall_;
01002 }
01003
01004
01005
01006
01007
01008 double BMC050::getUncompensatedYaw()
01009 {
01010
01011 return atan2( CompassX_, CompassY_ ) * ( 180.0 / M_PI );
01012 }
01013
01014
01015
01016
01017
01018 uint8_t BMC050::getDeviceAddress()
01019 {
01020
01021 ROS_ERROR("BMC050::getDeviceAddress(): Invalid request. Accelerometer and Compass have separate device addresses.");
01022 return 0;
01023 }
01024
01025
01026
01027
01028
01029 bool BMC050::enterLowPowerModeAccel()
01030 {
01031 switch( accel_mode_ )
01032 {
01033 case accel_low_power_:
01034 ROS_WARN("BMC050::enterLowPowerModeAccel(): sensor already in low-power mode.");
01035 break;
01036
01037 case accel_normal_: {}
01038 case accel_suspend_:
01039
01040
01041 modechange_accel_ = ( (1 << LOWPOWER_EN) | modechange_accel_ ) & ~(1 << SUSPEND);
01042
01043 if( this->writeToAccelReg(MODECHANGE_ACCEL, modechange_accel_ ) == false )
01044 {
01045 ROS_ERROR("BMC050::enterLowPowerModeAccel(): write failed.");
01046
01047 this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01048 return false;
01049 }
01050 break;
01051 default:
01052 ROS_ERROR("BMC050::enterLowPowerModeAccel(): failed.");
01053 return false;
01054 }
01055
01056 accel_mode_ = accel_low_power_;
01057 return true;
01058 }
01059
01060
01061
01062 bool BMC050::enterSuspendModeAccel()
01063 {
01064 switch( accel_mode_ )
01065 {
01066 case accel_suspend_:
01067 ROS_INFO("BMC050::enterSuspendModeAccel(): sensor already in suspend mode.");
01068 break;
01069
01070 case accel_normal_: {}
01071 case accel_low_power_:
01072
01073
01074 modechange_accel_ = ( (1 << SUSPEND) | modechange_accel_ ) & ~(1 << LOWPOWER_EN);
01075
01076 if( this->writeToAccelReg( MODECHANGE_ACCEL, modechange_accel_ ) == false )
01077 {
01078 ROS_ERROR("BMC050::enterSuspendModeAccel(): write failed.");
01079
01080 this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01081 return false;
01082 }
01083 break;
01084 default:
01085 ROS_ERROR("BMC050::enterSuspendModeAccel(): failed.");
01086 return false;
01087 }
01088
01089 accel_mode_ = accel_suspend_;
01090 return true;
01091 }
01092
01093
01094
01095 bool BMC050::enterNormalModeAccel()
01096 {
01097 switch( accel_mode_ )
01098 {
01099 case accel_normal_:
01100 ROS_WARN("BMC050::enterNormalModeAccel(): sensor already in normal mode.");
01101 break;
01102
01103 case accel_suspend_: {}
01104 case accel_low_power_:
01105
01106
01107 modechange_accel_ &= ~( (1 << SUSPEND) | (1 << LOWPOWER_EN) );
01108
01109 if( this->writeToAccelReg( MODECHANGE_ACCEL, modechange_accel_ ) == false )
01110 {
01111 ROS_ERROR("BMC050::enterNormalModeAccel(): write failed.");
01112
01113 this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01114 return false;
01115 }
01116 usleep( 800 );
01117 break;
01118 default:
01119 ROS_ERROR("BMC050::enterNormalModeAccel(): failed.");
01120 return false;
01121 }
01122
01123 accel_mode_ = accel_normal_;
01124 return true;
01125 }
01126
01127
01128
01129 bool BMC050::enterSuspendModeCompass()
01130 {
01131
01132 switch( compass_mode_ )
01133 {
01134 case compass_suspend_:
01135 ROS_WARN("BMC050::enterSuspendModeCompass(): device already in Suspend Mode.");
01136 break;
01137 case compass_forced_:
01138
01139 this->enterSleepModeCompass();
01140
01141 case compass_sleep_: {}
01142 case compass_normal_: {}
01143
01144 softreset_ &= (0 << POWER_CONTROL_BIT);
01145
01146 if( this->writeToCompassReg( SOFTRESET_COMPASS_REG, softreset_ ) == false )
01147 {
01148 ROS_ERROR("BMC050::enterSuspendModeCompass(): failed to write to registers.");
01149
01150 this->readRegCompass( SOFTRESET_COMPASS_REG, &softreset_ );
01151 return false;
01152 }
01153 break;
01154 default:
01155 ROS_ERROR( "BMC050::enterSuspendModeCompass(): failed to enter Suspend Mode." );
01156 return false;
01157 }
01158
01159 compass_mode_ = compass_suspend_;
01160 return true;
01161 }
01162
01163
01164
01165 bool BMC050::enterSleepModeCompass()
01166 {
01167
01168 switch( compass_mode_ )
01169 {
01170 case compass_normal_: {}
01171 case compass_forced_:
01172
01173 rate_compass_ |= (SET_OPMODE_SLEEP << OPMODE);
01174
01175 if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01176 {
01177 ROS_ERROR("BMC050::enterSleepModeCompass(): failed.");
01178
01179 this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01180 return false;
01181 }
01182 break;
01183 case compass_suspend_:
01184
01185 softreset_ = (1 << POWER_CONTROL_BIT);
01186
01187 if( this->writeToCompassReg( SOFTRESET_COMPASS_REG, softreset_ ) == false )
01188 {
01189 ROS_ERROR("BMC050::enterSleepModeCompass(): writing to registers failed.");
01190
01191 this->readRegCompass( SOFTRESET_COMPASS_REG, &softreset_ );
01192 return false;
01193 }
01194 usleep( 3000 );
01195 break;
01196 case compass_sleep_:
01197 ROS_WARN("BMC050::enterSleepModeCompass():Device already in sleep mode.");
01198 break;
01199 default:
01200 ROS_ERROR("BMC050::enterSleepModeCompass(): failed.");
01201 return false;
01202 }
01203
01204 compass_mode_ = compass_sleep_;
01205 return true;
01206 }
01207
01208
01209
01210 bool BMC050::enterNormalModeCompass()
01211 {
01212
01213 switch( compass_mode_ )
01214 {
01215
01216 case compass_suspend_: {}
01217 case compass_forced_:
01218
01219 this->enterSleepModeCompass();
01220
01221 case compass_sleep_:
01222
01223 rate_compass_ &= ~(SET_OPMODE_SLEEP << OPMODE);
01224
01225 if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01226 {
01227 ROS_ERROR("BMC050::enterNormalModeCompass(): failed to write to registers.");
01228
01229 this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01230 return false;
01231 }
01232 break;
01233 case compass_normal_:
01234
01235 ROS_WARN("BMC050::enterNormalModeCompass(): sensor already in normal mode.");
01236 break;
01237 default:
01238 ROS_ERROR("BMC050::enterNormalModeCompass(): error entering Normal Mode.");
01239 return false;
01240 }
01241
01242 compass_mode_ = compass_normal_;
01243 return true;
01244 }
01245
01246
01247
01248 bool BMC050::enterForcedModeCompass()
01249 {
01250
01251 switch( compass_mode_ )
01252 {
01253 case compass_forced_:
01254 ROS_WARN("BMC050::enterForcedModeCompass(): device already in forced mode.");
01255 break;
01256
01257 case compass_suspend_: {}
01258 case compass_normal_: {}
01259 this->enterSleepModeCompass();
01260 case compass_sleep_:
01261
01262 rate_compass_ &= ~(SET_OPMODE_SLEEP << OPMODE);
01263 rate_compass_ |= (SET_OPMODE_FORCED << OPMODE );
01264
01265 if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01266 {
01267 ROS_ERROR("BMC050::enterForcedModeCompass(): failed to write to registers.");
01268
01269 this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01270 return false;
01271 }
01272 break;
01273 default:
01274 ROS_ERROR("BMC050::enterForcedModeCompass(): failed.");
01275 return false;
01276 }
01277 compass_mode_ = compass_forced_;
01278 return true;
01279 }
01280
01281
01282
01283
01284
01285 bool BMC050::readRegAccel( uint8_t reg, uint8_t* value )
01286 {
01287
01288 switch( this->getProtocol() )
01289 {
01290 case I2C:
01291 if( hardware_->read( this->getAccelAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 )
01292 {
01293 ROS_ERROR("BMC050::readRegAccel(): Error reading register via I2C!");
01294 return false;
01295 }
01296 break;
01297 case SPI:
01298
01299 if( hardware_->read( this->getAccelAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, value, 1 ) < 0 )
01300 {
01301 ROS_ERROR("BMC050::readRegAccel(): Error reading register via SPI!");
01302 return false;
01303 }
01304 break;
01305 default:
01306 ROS_ERROR("BMC050::readRegAccel(...): invalid protocol.");
01307 return false;
01308 }
01309 return true;
01310 }
01311
01312
01313
01314
01315
01316 bool BMC050::readRegCompass( uint8_t reg, uint8_t* value )
01317 {
01318 switch( this->getProtocol() )
01319 {
01320 case I2C:
01321 if( hardware_->read( this->getCompassAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 )
01322 {
01323 ROS_ERROR("BMC050::readRegCompass(): Error reading register via I2C!");
01324 return false;
01325 }
01326 break;
01327 case SPI:
01328
01329 if (hardware_->read( this->getCompassAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, value, 1 ) < 0 )
01330 {
01331 ROS_ERROR("BMC050::readRegCompass(): Error reading register via SPI!");
01332 return false;
01333 }
01334 break;
01335 default:
01336 ROS_ERROR("BMC050::readRegistersCompass(...): invalid protocol.");
01337 return false;
01338 }
01339 return true;
01340 }
01341
01342
01343
01344
01345
01346 bool BMC050::writeToAccelReg( uint8_t reg, uint8_t value )
01347 {
01348 switch( this->getProtocol() )
01349 {
01350 case I2C:
01351 if( hardware_->write( this->getAccelAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0 )
01352 {
01353 ROS_ERROR("BMC050::writeToAccelReg(): Error writing to register via I2C!");
01354 return false;
01355 }
01356 break;
01357 case SPI:
01358
01359 if( hardware_->write( this->getAccelAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)®), (uint8_t*)&value, 1 ) < 0 )
01360 {
01361 ROS_ERROR("BMC050::writeToAccelReg(): Error writing to register via SPI!");
01362 return false;
01363 }
01364 break;
01365 default:
01366 ROS_ERROR("BMC050::writeToAccelReg(...): invalid protocol.");
01367 return false;
01368 }
01369 return true;
01370 }
01371
01372
01373
01374
01375
01376 bool BMC050::writeToCompassReg( uint8_t reg, uint8_t value )
01377 {
01378 switch( this->getProtocol() )
01379 {
01380 case I2C:
01381 if( hardware_->write( this->getCompassAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0)
01382 {
01383 ROS_ERROR("BMC050::writeToCompassReg(): Error writing to register via I2C!");
01384 return false;
01385 }
01386 break;
01387 case SPI:
01388
01389 if( hardware_->write( this->getCompassAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)®), (uint8_t*)&value, 1 ) < 0 )
01390 {
01391 ROS_ERROR("BMC050::writeToCompassReg(): Error writing to register via SPI!");
01392 return false;
01393 }
01394 break;
01395 default:
01396 ROS_ERROR("BMC050::writeToCompassReg(...): invalid protocol.");
01397 return false;
01398 }
01399 return true;
01400 }
01401
01402
01403
01404
01405
01406 bool BMC050::writeToCompassRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01407 {
01408 this->writeToCompassReg( reg, value );
01409
01410 uint8_t actual;
01411
01412 this->readRegCompass( reg, &actual );
01413
01414 if( expected != actual )
01415 {
01416 ROS_ERROR("BMC050::writeToCompassRegAndVerify(...): Compass Register: %d actual: %d expected: %d ", reg, actual, expected);
01417 return false;
01418 }
01419
01420 return true;
01421 }
01422
01423
01424
01425
01426
01427 bool BMC050::writeToAccelRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01428 {
01429 this->writeToAccelReg( reg, value );
01430
01431 uint8_t actual;
01432
01433 this->readRegAccel( reg, &actual );
01434
01435 if( expected != actual )
01436 {
01437 ROS_ERROR("BMC050::writeToAccelRegAndVerify(...): Accelerometer Register: %d actual: %d expected: %d", reg, actual, expected);
01438 return false;
01439 }
01440
01441 return true;
01442 }
01443
01444
01445
01446
01447 bool BMC050::printOffsets()
01448 {
01449 uint8_t reg_to_print;
01450 std::string offset_reg_names[6] = { "offset_filt_x", "offset_filt_y", "offset_filt_z", "offset_unfilt_x", "offset_unfilt_y", "offset_unfilt_z" };
01451
01452 for( uint8_t i = OFFSET_FILT_X; i <= OFFSET_UNFILT_Z; i++ )
01453 {
01454 if( this->readRegAccel( i, ®_to_print ) == false )
01455 return false;
01456
01457 std::cout << offset_reg_names[i - OFFSET_FILT_X] << ": " << (int)reg_to_print << std::endl;
01458 }
01459 return true;
01460 }