bma180.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Robert Bosch LLC
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Robert Bosch LLC nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*********************************************************************
00031  *
00032  * Disclaimer
00033  *
00034  * This source code is not officially released or supported by Bosch Sensortec.
00035  * Please contact the listed authors with bugs or other modifications.
00036  * If you would like the official Bosch Sensortec drivers, please contact:
00037  * contact@bosch-sensortec.com
00038  *
00039  *********************************************************************/
00040 
00041 
00042 /*
00043  * bma180.cpp
00044  *
00045  *  Created on: Jul 26, 2011
00046  *      Author: Lucas Marti, Robert Bosch LLC
00047  *      Editor: Nikhil Deshpande, Philip Roan
00048  */
00049 
00050 // Standard headers
00051 #include <iostream>
00052 #include <string>
00053 #include <sstream>
00054 #include <math.h>
00055 #include <vector>
00056 // ROS headers
00057 #include <ros/ros.h>
00058 #include <ros/time.h>
00059 // ROS messages
00060 #include "bma180/bma180.h"
00061 
00062 
00063 Bma180::Bma180( double dMaxAcc_g, double dBandwidth_Hz, bool bCalibrate, double dRate_Hz, std::string sSub20Serial ):
00064   bSubDeviceOpen       (false),
00065   bSubDeviceConfigured (false)
00066 {
00067   int               iSpiErr;      // Error code for std Sub20 API
00068   int               iSPI_cfg_set; // Set SPI config
00069   int               iSPI_cfg_get; // Get SPI config
00070   int               iADCErr;      // Error code for std Sub20 API
00071   int               iADC_cfg_set; // Set ADC config
00072   int               iNumOfRanges;
00073   int               iElCount;
00074   std::stringstream ss_errmsg;
00075   std::string       ss_config, sub20Serial;
00076   sub_device        sub20dev;
00077   sub_handle        sub20handle;
00078   OneSub20Config    OneSub20Configuration;
00079 
00080   ROS_INFO("--------------------------------------");
00081   ROS_INFO("Max acceleration entered:: %f ", dMaxAcc_g);
00082   ROS_INFO("Sensor bandwidth entered:: %f ", dBandwidth_Hz);
00083   ROS_INFO("Sensor Reading Rate entered:: %f ", dRate_Hz);
00084   ROS_INFO("--------------------------------------");
00085 
00086   //Retrieve calibration data from user
00087   set_calibstatus( bCalibrate );
00088 
00089   // Evaluate the accelerometer range  to be selected
00090   iNumOfRanges = sizeof(bma180_cmd::chCMD_FULLSCALE_G) / sizeof(char);
00091   iElCount = 0;
00092   while( (dMaxAcc_g > bma180_cmd::dFULLRANGELOOKUP[iElCount]) && (iElCount < iNumOfRanges) )
00093   {
00094     iElCount++;
00095   };
00096   ROS_INFO("Range chosen G:: %f ", bma180_cmd::dFULLRANGELOOKUP[iElCount]);
00097 
00098   chMaxAccRange_selected = bma180_cmd::chCMD_FULLSCALE_G[iElCount];
00099 
00100   // Evaluate the bandwidth to be selected
00101   iNumOfRanges = sizeof(bma180_cmd::chCMD_BANDWIDTH_HZ) / sizeof(char);
00102   iElCount = 0;
00103   while( (dBandwidth_Hz > bma180_cmd::dBWLOOKUP[iElCount]) && (iElCount < iNumOfRanges) )
00104   {
00105     iElCount++;
00106   };
00107   ROS_INFO("Range chosen BW:: %f ", bma180_cmd::dBWLOOKUP[iElCount]);
00108 
00109   chBW_selected = bma180_cmd::chCMD_BANDWIDTH_HZ[iElCount];
00110 
00111   // Open connected Sub20 Devices
00112   sub20dev = 0;
00113   sub20dev = sub_find_devices( sub20dev );
00114   while( sub20dev != 0 )
00115   {
00116     sub20handle = sub_open( sub20dev );
00117     // on success, sub_open returns non-zero handle
00118     if( sub20handle > 0 )
00119     {
00120       sub20Serial.clear();
00121       sub20Serial.resize( bma180_cmd::uSERIALNUMLENGTH );
00122       sub_get_serial_number( sub20handle, const_cast<char*>(sub20Serial.c_str()), sub20Serial.size() );
00123       std::cout << "Serial Number   : " << sub20Serial << std::endl;
00124       if( strcmp(sub20Serial.c_str(), sSub20Serial.c_str()) == 0 )
00125       {
00126         subhndl = sub20handle;
00127 
00128         // Configure Sub20 device
00129         std::cout << "---Initializing SPI Interface---" << " \n";
00130         // Read current SPI configuration
00131         iSpiErr = sub_spi_config( sub20handle, 0, &iSPI_cfg_get );
00132         std::cout << "Sub SPI config  : " << iSPI_cfg_get << " \n";
00133         //Important: The SPI interface does not work properly at higher frequencies, i.e. > 2MHz
00134         // SET: Polarity Fall, SetupSmpl,  MSB first, 2MHZ
00135         iSPI_cfg_set = SPI_ENABLE|SPI_CPOL_FALL|SPI_SETUP_SMPL|SPI_MSB_FIRST|SPI_CLK_2MHZ;
00136         //Configure SPI
00137         iSpiErr = sub_spi_config( sub20handle, iSPI_cfg_set, 0 );
00138         //Read current SPI configuration
00139         iSpiErr = sub_spi_config( sub20handle, 0, &iSPI_cfg_get );
00140         //verify if sub20 device has accepted the configuration
00141         if( iSPI_cfg_get == iSPI_cfg_set )
00142         {
00143           std::cout<< "SPI Configuration   : " << iSPI_cfg_set << " successfully stored \n";
00144           //Subdevice has been configured successfully
00145           OneSub20Configuration.bSubSPIConfigured = true;
00146           OneSub20Configuration.handle_subdev = sub20handle;
00147         
00148           // Configure Sensors on Sub20
00149           confsens_on_sub20( &OneSub20Configuration, chMaxAccRange_selected, chBW_selected );
00150         }
00151         else
00152         {
00153           ROS_INFO("ERROR - SPI Configuration : %d not accepted by device", iSPI_cfg_set);
00154           //Subdevice could not be configured
00155           OneSub20Configuration.bSubSPIConfigured = false;
00156         }
00157         if( OneSub20Configuration.bSubSPIConfigured )
00158         {
00159           //string needs to be cleared otherwise conversion is going wrong
00160           strSerial.clear();
00161           strSerial.resize( bma180_cmd::uSERIALNUMLENGTH );
00162           sub_get_serial_number( sub20handle, const_cast<char*>(strSerial.c_str()), strSerial.size() );
00163           OneSub20Configuration.strSub20Serial = strSerial;
00164           OneSub20Configuration.subdev = sub20dev;
00165           std::cout << "Device Handle   : " << OneSub20Configuration.handle_subdev << std::endl;
00166           std::cout << "Serial Number   : " << OneSub20Configuration.strSub20Serial << std::endl;
00167 
00168           // Push element onto list of subdevices
00169           Sub20Device_list.push_back( OneSub20Configuration );
00170           std::cout << "... Publishing to topic /bma180 ... " << std::endl;
00171         }
00172         break;
00173       } 
00174       else
00175       {
00176         sub_close( sub20handle );
00177       }
00178     }
00179     //find next device, sub_find_devices using current provides next
00180     sub20dev = sub_find_devices( sub20dev );
00181   }
00182 };
00183 
00184 Bma180::~Bma180()
00185 {
00186   int iSpiErr;
00187   OneSub20Config OneSub20Configuration;
00188   //close opened subdevices
00189   // Disable SPI
00190   iSpiErr = sub_spi_config( subhndl, 0, 0 );
00191   // Close USB device
00192   sub_close( subhndl );
00193   while( !Sub20Device_list.empty() )
00194   {
00195     OneSub20Configuration = Sub20Device_list.back();
00196     std::cout << "Sub device removed " << OneSub20Configuration.strSub20Serial << "\n";
00197     Sub20Device_list.pop_back();
00198   }
00199 };
00200 
00201 void Bma180::GetMeasurements( std::list<OneBma180Meas> &list_meas )
00202 {
00203   char chACC_XYZ[7]; //Containing
00204   double dAccX, dAccY, dAccZ;
00205   double dEstBiasX, dEstBiasY, dEstBiasZ;
00206   int uiBcorrX, uiBcorrY, uiBcorrZ, uiRawX, uiRawY, uiRawZ;
00207   double dTemp;
00208   int iSpiErr, iADCErr, dummy, j = 0;
00209   bool SPIMeas = false, ADCMeas = false;
00210   OneBma180Meas sMeas;
00211   ros::Time dtomeas;
00212   char chCMD;
00213   std::stringstream ss_errmsg;
00214   std::list<OneSub20Config>::iterator iterat;
00215   int iChipSelect;
00216 
00217   //verify that a subdevice is connected
00218   if( 1 > (int)Sub20Device_list.size() )
00219   {
00220     throw std::string ("No SubDevice connected OR access rights to USB not given");
00221   }
00222 
00223   //Trace sub20 list
00224   for( iterat = Sub20Device_list.begin(); iterat != Sub20Device_list.end(); iterat++ )
00225   {
00226     //verify if the sub20 device is configured (which should be the case as only configured sub20ies are pushed on list)
00227     if( iterat->bSubSPIConfigured == true )
00228     {
00229       //Trace through cluster
00230       for( iChipSelect = 0; iChipSelect < bma180_cmd::iMAXNUM_OF_SENSORS; iChipSelect++ )
00231       {
00232       //verify if sensor is available on respective chipselect
00233         if( iterat->Bma180Cluster[iChipSelect].bConfigured == true )
00234         {
00235           iSpiErr = 0;
00236           //NOTE: Not executed in order to save one SPI transfer
00237           // //ensure CS is high
00238           // //iSpiErr = sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00239           // //send read command
00240           chCMD = bma180_cmd::chADR_ACCLXYZ | bma180_cmd::chFLAG_R;
00241           iSpiErr += sub_spi_transfer( iterat->handle_subdev, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00242           iSpiErr += sub_spi_transfer( iterat->handle_subdev, 0, chACC_XYZ, 7, SS_CONF(iChipSelect,SS_L) );
00243           //ensure CS is high
00244           iSpiErr += sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00245           
00246           if( iSpiErr == 0 )
00247           {
00248             SPIMeas = true;
00249             dAccX = bma180data_to_double(chACC_XYZ[1], chACC_XYZ[0], bma180_cmd::eACCEL, &uiRawX, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00250             dAccY = bma180data_to_double(chACC_XYZ[3], chACC_XYZ[2], bma180_cmd::eACCEL, &uiRawY, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00251             dAccZ = bma180data_to_double(chACC_XYZ[5], chACC_XYZ[4], bma180_cmd::eACCEL, &uiRawZ, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00252             dTemp = bma180data_to_double(chACC_XYZ[6], chACC_XYZ[6], bma180_cmd::eTEMP,  &dummy, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00253 
00254             // Collect data for all chipSelects
00255             sMeas.dAccX[j] = dAccX;
00256             sMeas.dAccY[j] = dAccY;
00257             sMeas.dAccZ[j] = dAccZ;
00258             sMeas.dTemp    = dTemp;
00259             sMeas.iChipSelect[j] = iChipSelect;
00260             j++;
00261             sMeas.iNumAccels = j;
00262 
00263             // Execute calibration if desired
00264             // Note: For calibration procedure, only one XDIMAX box is allowed to be connected
00265             if( (bExecuteCalibration == true) && (2>(int)Sub20Device_list.size()) )
00266             {
00267               if( iChipSelect == iCalib_CS )
00268               {
00269                 if( (calib_bma180.calibsens_completed() == false) && (calib_bma180.calibsens_set() == false) )
00270                 {
00271                   std::cout << " Bias read from image : " << iterat->Bma180Cluster[iChipSelect].uiBiasImageX << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageY << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageZ << std::endl;
00272                   //set the sensor to be calibrated
00273                   calib_bma180.setcalibsens( sMeas );
00274                 }
00275                 else
00276                 {
00277                   calib_bma180.setdata_bma180(sMeas);
00278                 }
00279                 if( (calib_bma180.calibsens_completed())&&(!calib_bma180.verification_active()) )
00280                 {
00281                   // Get estimated sensor biases
00282                   calib_bma180.get_estbiases( &dEstBiasX, &dEstBiasY, &dEstBiasZ );
00283                   std::cout << "estimated biases : " << dEstBiasX << " " << dEstBiasY << " " << dEstBiasZ << std::endl;
00284 
00285                   // calculate adjustment
00286                   // +0.5 for typecast to get propper rounding
00287                   // 2048: Bias has 12 bit, thus halfrange is 2^11 where as bias corr scales by range
00288                   int iBcorrX, iBcorrY, iBcorrZ;
00289                   iBcorrX = ((short)(dEstBiasX/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5));
00290                   iBcorrY = ((short)(dEstBiasY/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5)) ;
00291                   iBcorrZ = ((short)((dEstBiasZ-1)/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5)) ;
00292                   std::cout << "Bias adjustments [X Y Z] " << iBcorrX << " " << iBcorrY << " " << iBcorrZ << std::endl;
00293                   std::cout << "Image values before adjustments " << iterat->Bma180Cluster[iChipSelect].uiBiasImageX << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageY << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageZ << std::endl;
00294                   uiBcorrX = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageX - ((short)(dEstBiasX/bma180_cmd::dFULLRANGELOOKUP[6]*2048)) );
00295                   uiBcorrY = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageY - ((short)(dEstBiasY/bma180_cmd::dFULLRANGELOOKUP[6]*2048)) );
00296                   uiBcorrZ = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageZ - ((short)((dEstBiasZ-1)/bma180_cmd::dFULLRANGELOOKUP[6]*2048)));
00297 
00298                   std::cout << "corrected biases " << uiBcorrX << " " << uiBcorrY << " " << uiBcorrZ << std::endl;
00299 
00300                   // Write to image
00301                   if( !set_biassettings(iterat->handle_subdev, iChipSelect, uiBcorrX, uiBcorrY, uiBcorrZ, false) )
00302                   {
00303                     bExecuteCalibration = false;
00304                     throw std::string("Bias corrections cannot be written to image - Calibration procedure aborted");
00305                   }
00306 
00307                   // Activate verification
00308                   if( !calib_bma180.biasverify() )
00309                   {
00310                     bExecuteCalibration = false;
00311                     throw std::string("BiasVerification cannot be executed - Calibration procedure aborted");
00312                   }
00313                 }
00314                 
00315                 if( (calib_bma180.calibsens_completed()) && (calib_bma180.verification_active()) && (calib_bma180.verification_completed()) )
00316                 {
00317                   std::cout << "-----------------------------------------" << std::endl;
00318                   std::cout << "...............VERIFY BIASES............." << std::endl;
00319                   std::cout << "-----------------------------------------" << std::endl;
00320 
00321                   // Get verified biases after image write
00322                   if( !calib_bma180.get_verifiedbiases(&dEstBiasX, &dEstBiasY, &dEstBiasZ) )
00323                   {
00324                     bExecuteCalibration = false;
00325                     throw std::string("Verified biases cannot be retrieved - Calibration procedure aborted");
00326                   };
00327                   
00328                   if( (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasX)) && (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasY)) && (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasZ-1)) )
00329                   {
00330                     // Writing EEPROM
00331                     uiBcorrX = 0;
00332                     uiBcorrY = 0;
00333                     uiBcorrZ = 0;
00334                     std::string myAnswer;
00335                     std::cin.ignore();
00336                     std::cout << "Really write EEPROM <Y/N>? ";
00337                     std::getline( std::cin, myAnswer );
00338                     if( (0 == myAnswer.find('Y')) || (0 == myAnswer.find('y')) )
00339                     {
00340                       if( !set_biassettings(iterat->handle_subdev, iChipSelect, uiBcorrX, uiBcorrY, uiBcorrZ, true) )
00341                       {
00342                         bExecuteCalibration = false;
00343                         throw std::string("Biases cannot be written into EEPROM - Calibration procedure aborted");
00344                       };
00345                       ROS_INFO("++++++EEPROM HAS BEEN WRITTEN+++++++");
00346 
00347                     }
00348                     else
00349                     {
00350                       ROS_INFO("++++++EEPROM WRITE ABORTED++++++");
00351                     }
00352                   }
00353                   else
00354                   {
00355                     std::cout << "Error values " << fabs(dEstBiasX) << " " << fabs(dEstBiasY) << " " << fabs(dEstBiasZ-1) << std::endl;
00356                     bExecuteCalibration = false;
00357                     throw std::string("Bias verification failed, calib values too large - Calibration procedure aborted");
00358                   }
00359                   //We are done with calibration
00360                   bExecuteCalibration = false;
00361                   std::cout << "calibration completed hehe" << std::endl;
00362                 }
00363               }
00364             }
00365             else
00366             {
00367               if( (bExecuteCalibration == true) && (1<(int)Sub20Device_list.size()) )
00368               {
00369                 throw std::string("As a pre-caution: For calibration, only ONE SubDevice must be connected");
00370               }
00371             }
00372           }
00373           else
00374           {
00375             throw std::string("Error on SPI communication - keep on running though");
00376           }
00377         } //config verify
00378       } //scanning through all the chipselects
00379     }
00380     // Only publish data after all data for all chipSelects is collected
00381     if( SPIMeas )
00382     {
00383       sMeas.dtomeas        = ros::Time::now();
00384       sMeas.bMeasAvailable = true;
00385       sMeas.strSerial      = iterat->strSub20Serial;
00386       //Push measurement onto heap
00387       list_meas.push_back( sMeas );
00388     }
00389     else 
00390     {
00391       throw std::string("SUB20 connected but not configured - Restart of node suggested");
00392     }
00393   } //sub20 for loop
00394 };
00395 
00396 double Bma180::bma180data_to_double( char chMSB, char chLSB, bma180_cmd::eSensorType eSensor, int *raw, double dAccScale )
00397 {
00398   short  s16int;      // 2 byte int to build message
00399   double dSensorMeas; // Scaled sensor measurement
00400 
00401   switch( eSensor )
00402   {
00403   // Retrieve Accel Data
00404   case bma180_cmd::eACCEL:
00405     //verify if pos or neg
00406     if( (chMSB & 0x80) == 0 )
00407     {
00408       //positive number
00409       s16int = (short) ((((unsigned short)chMSB)&0xFF)<<6)+((((unsigned short)chLSB)&0xFC)>>2);
00410     }
00411     else
00412     {
00413       //negative number
00414       //set MSB (sign) to zero and build 2 complement unsigned; offset 8192 for 2^13
00415       s16int = (short) (((((unsigned short)chMSB)&0x7F)<<6)+((((unsigned short)chLSB)&0xFC)>>2) - 8192);
00416     }
00417     dSensorMeas = (( (double) s16int) / 8192)*dAccScale;
00418     break;
00419 
00420   // Convert Temperature Data
00421   case bma180_cmd::eTEMP:
00422     //verify if pos or neg
00423     if( (chLSB & 0x80) == 0 )
00424     {
00425       //positive number
00426       s16int = (short) (((unsigned short)chLSB)&0xFF);
00427     }
00428     else
00429     {
00430       //negative number
00431       //set MSB (sign) to zero and build 2 complement unsigned; offset 128 for 2^7
00432       s16int = (short) ((((unsigned short)chLSB)&0x7F) - 128 );
00433     };
00434     dSensorMeas = (( (double) s16int) / 8192)*2;
00435     break;
00436 
00437   default :
00438     dSensorMeas = 0;
00439   }
00440   *raw = s16int;
00441   return dSensorMeas;
00442 };
00443 
00444 unsigned short Bma180::bma180data_to_uint( char chMSB, char chLSB, bma180_cmd::eSensorType eSensor )
00445 {
00446   unsigned short u16int_bias; //2 byte int to build message
00447   
00448   switch (eSensor)
00449   {
00450   // Bias X axis data
00451   case bma180_cmd::eBIAS_X:
00452     u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0xF0)>>4);
00453     break;
00454   // Bias Y axis data
00455   case bma180_cmd::eBIAS_Y:
00456     u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0x0F));
00457     break;
00458   // Bias Z axis data
00459   case bma180_cmd::eBIAS_Z:
00460     u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0xF0)>>4);
00461     break;
00462   default :
00463     u16int_bias = 0;
00464   }
00465   return u16int_bias;
00466 };
00467 
00468 bool Bma180::read_byte_eeprom_sub20( char chADR, char* pchREGISTER, unsigned short iChipSelect, sub_handle sub_h )
00469 {
00470   int  iSpiErr = 0;
00471   bool bSuccess;
00472   char chCMD;
00473 
00474   // Read register at specified adderss chADR
00475   //ensure CS is high
00476   iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00477   //send read command
00478   chCMD = chADR | bma180_cmd::chFLAG_R;
00479   iSpiErr += sub_spi_transfer( sub_h, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00480   iSpiErr += sub_spi_transfer( sub_h, 0, pchREGISTER, 1, SS_CONF(iChipSelect,SS_L) );
00481   //ensure CS is high
00482   iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00483 
00484   if( iSpiErr == 0)
00485   {
00486     //successful operation
00487     bSuccess = true;
00488   }
00489   else
00490   {
00491     //SPI error
00492     bSuccess = false;
00493   }
00494   return bSuccess;
00495 };
00496 
00497 bool Bma180::write_bit_eeprom_sub20( char chADR, unsigned short iLSBOfByte, unsigned short iNumOfBits, char chBitSeq, unsigned short iChipSelect, sub_handle sub_h )
00498 {
00499   char chCMD; //temporary character for building commands
00500   char chREGISTER; //temporary read status of register
00501   char chREGISTER_MODIFIED;
00502   int  iSpiErr;
00503   char chMask;
00504   bool bSuccess;
00505   bool bEEReadSuccess;
00506 
00507   //only one byte will be handled, thus iLSBOfByte + iNumOfBits > 8 means out of range
00508   if( iLSBOfByte + iNumOfBits <= 8 )
00509   {
00510     iSpiErr = 0;
00511     bEEReadSuccess = true;
00512 
00513     // Read register at specified address chADR
00514     chCMD = chADR | bma180_cmd::chFLAG_R;
00515     bEEReadSuccess &= read_byte_eeprom_sub20(chCMD, &chREGISTER, iChipSelect, sub_h );
00516 
00517     // Write the specified bit into register
00518     //Build mask, i.e. zero pad the respective points where we need to have
00519     if ( iNumOfBits < 8 )
00520     {
00521       chMask = ~( ((1<<iNumOfBits)-1)<<iLSBOfByte );
00522     }
00523     else
00524     {
00525       //as function of the range check above, it can only be 8
00526       chMask = 0x00;
00527     }
00528     //write specific register
00529     chREGISTER_MODIFIED = (chREGISTER&chMask ) | ( chBitSeq << iLSBOfByte );
00530     //build up command to be written to register (note read flag is not set
00531     chCMD  = chADR;
00532     iSpiErr += sub_spi_transfer( sub_h, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00533     iSpiErr += sub_spi_transfer( sub_h, &chREGISTER_MODIFIED, 0, 1, SS_CONF(iChipSelect,SS_L) );
00534     //ensure CS is high
00535     iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00536 
00537     // Check if register has been set (only verification)
00538     //send read command
00539     chCMD = chADR | bma180_cmd::chFLAG_R;
00540     bEEReadSuccess &= read_byte_eeprom_sub20(chCMD, &chREGISTER, iChipSelect, sub_h);
00541 
00542     if( (iSpiErr == 0) && (bEEReadSuccess) )
00543     {
00544       bSuccess = true;
00545     }
00546     else
00547     {
00548       //SPI communication error
00549       bSuccess = false;
00550     }
00551   }
00552   else
00553   {
00554     //out of range
00555     bSuccess = false;
00556   }
00557   return bSuccess;
00558 };
00559 
00560 void Bma180::confsens_on_sub20( OneSub20Config *pOneSub20Conf, char chFullscale, char chBandwidth )
00561  {
00562   int               iSpiErr;            //Error code for Sub20 API
00563   int               iChipSelect;
00564   bool              bSuccess;
00565   char              chCHIP_ID;
00566   char              chALML_VER;
00567   char              chCMD;
00568   char              chREGSTATUS;
00569   unsigned short    uiBiasX;
00570   unsigned short    uiBiasY;
00571   unsigned short    uiBiasZ;
00572   std::stringstream ss_errmsg;
00573   sub_handle        handle_sub20;
00574   bool              bEE_RWsuccess;
00575 
00576   //retrieve handle for sub20 device
00577   handle_sub20 = pOneSub20Conf->handle_subdev;
00578   for( iChipSelect = 0; iChipSelect < bma180_cmd::iMAXNUM_OF_SENSORS; iChipSelect++ )
00579   {
00580     pOneSub20Conf->Bma180Cluster[iChipSelect].iNumOfCalibMeas = 0;
00581     iSpiErr = 0;
00582     bEE_RWsuccess = true;
00583     //ensure CS is high
00584     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00585     //send read command
00586     chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00587     iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00588     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00589     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00590     //ensure CS is high
00591     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00592 
00593     //Verify if a BMA180 is connected on the respective ChipSelect
00594     if( (iSpiErr == 0) && (0<(unsigned short)chCHIP_ID) && (0<(unsigned short)chALML_VER) )
00595     {
00596       ROS_INFO("-----------------------------------------");
00597       ROS_INFO("BMA 180 - Chip Select %d", iChipSelect);
00598       ROS_INFO("CHIP ID    : %u ", (unsigned short)chCHIP_ID);
00599       ROS_INFO("ALML Ver   : %u ", (unsigned short)chALML_VER);
00600 
00601       //call method to set ee_write flag
00602       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_CTRLREG0, 4, 1, bma180_cmd::chCMD_SET_EEW, iChipSelect, handle_sub20 );
00603       //issue soft reset
00604       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_SOFTRESET, 0, 8, bma180_cmd::chCMD_SOFTRESET, iChipSelect, handle_sub20 );
00605       //call method to set ee_write flag
00606       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_CTRLREG0, 4, 1, bma180_cmd::chCMD_SET_EEW, iChipSelect, handle_sub20 );
00607       //change maxrange of sensor
00608       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_RANGE, 1, 3, chFullscale, iChipSelect, handle_sub20 );
00609       bEE_RWsuccess &= read_byte_eeprom_sub20( bma180_cmd::chADR_RANGE, &chREGSTATUS, iChipSelect, handle_sub20 );
00610       //Extract range
00611       pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange = bma180_cmd::dFULLRANGELOOKUP[((unsigned short)((chREGSTATUS & 0x0E)>>1))];
00612       //change sensor bandwidth
00613       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_BWTCS, 4, 4, chBandwidth, iChipSelect, handle_sub20 );
00614       bEE_RWsuccess &= read_byte_eeprom_sub20( bma180_cmd::chADR_BWTCS, &chREGSTATUS, iChipSelect, handle_sub20 );
00615       //Extract range
00616       pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth = bma180_cmd::dBWLOOKUP[((unsigned short)((chREGSTATUS & 0xF0)>>4))];
00617       ROS_INFO("eeprom stored Fullrange : %f  g ", pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange);
00618       ROS_INFO("eeprom stored Bandwidth : %f  Hz", pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth);
00619       ROS_INFO("-----------------------------------------");
00620 
00621       //Read bias that are currently stored in image
00622       bEE_RWsuccess &= read_biassettings( handle_sub20, iChipSelect, &uiBiasX, &uiBiasY, &uiBiasZ );
00623       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageX = uiBiasX;
00624       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageY = uiBiasY;
00625       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageZ = uiBiasZ;
00626 
00627       if( bEE_RWsuccess )
00628       {
00629         //Set flag that sensor has been initialized
00630         pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured = true;
00631         //successful operation
00632         bSuccess = true;
00633       }
00634       else
00635       {
00636         //Set flag that sensor has been initialized
00637         pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured = false;
00638         //successful operation
00639         bSuccess = false;
00640       }
00641     }
00642     else 
00643     {
00644       ROS_INFO("-----------------------------------------");
00645       ROS_INFO(" BMA 180 - Chip Select %d ", iChipSelect);
00646       ROS_INFO(" NO SENSOR DETECTED ");
00647       ROS_INFO("-----------------------------------------");
00648 
00649       //Set flag that sensor hasn't been initialized
00650       pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured = false;
00651       pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange = 0;
00652       pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth = 0;
00653       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageX = 0;
00654       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageY = 0;
00655       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageZ = 0;
00656       //unsuccessful operation
00657       bSuccess = false;
00658     }
00659   }
00660 };
00661 
00662 bool Bma180::read_biassettings( sub_handle handle_sub20, int iChipSelect, unsigned short* uiBiasX, unsigned short* uiBiasY, unsigned short* uiBiasZ )
00663 {
00664   int  iSpiErr;
00665   char chBiasXYZT[6];
00666   bool bSuccess;
00667   char chCMD;
00668   char chCHIP_ID;
00669   char chALML_VER;
00670 
00671   // Ensure BMA180 is connected
00672   //ensure CS is high
00673   iSpiErr = 0;
00674   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00675 
00676   //send read command
00677   chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00678   iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00679   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00680   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00681 
00682   //ensure CS is high
00683   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00684 
00685   //Verify if a BMA180 is connected on the respective ChipSelect
00686   if( (iSpiErr == 0) && (0 < (unsigned short)chCHIP_ID) && (0 < (unsigned short)chALML_VER) )
00687   {
00688     //ensure CS is high
00689     //NOTE: Not executed in order to save one SPI transfer
00690     //iSpiErr = sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00691     //send read command
00692     chCMD = bma180_cmd::chADR_OFFSET_LSB1 | bma180_cmd::chFLAG_R;
00693     iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00694     iSpiErr += sub_spi_transfer( handle_sub20, 0, chBiasXYZT, 6, SS_CONF(iChipSelect,SS_L) );
00695     //ensure CS is high
00696     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00697 
00698     if( iSpiErr == 0 )
00699     {
00700       (*uiBiasX) = bma180data_to_uint( chBiasXYZT[3], chBiasXYZT[0], bma180_cmd::eBIAS_X );
00701       (*uiBiasY) = bma180data_to_uint( chBiasXYZT[4], chBiasXYZT[1], bma180_cmd::eBIAS_Y );
00702       (*uiBiasZ) = bma180data_to_uint( chBiasXYZT[5], chBiasXYZT[1], bma180_cmd::eBIAS_Z );
00703       bSuccess = true;
00704     }
00705     else
00706     {
00707       *uiBiasX = 0;
00708       *uiBiasY = 0;
00709       *uiBiasZ = 0;
00710       bSuccess = false;
00711     }
00712   }
00713   else
00714   {
00715      *uiBiasX = 0;
00716      *uiBiasY = 0;
00717      *uiBiasZ = 0;
00718      bSuccess = false;
00719   }
00720   return bSuccess;
00721 };
00722 
00723 bool Bma180::set_biassettings( sub_handle handle_sub20, int iChipSelect, unsigned short uiBiasX, unsigned short uiBiasY, unsigned short uiBiasZ, bool bWriteEEPROM )
00724 {
00725   int  iSpiErr; //Error code for Sub20 API
00726   bool bSuccess;
00727   char chCMD;
00728   char chCHIP_ID;
00729   char chALML_VER;
00730   char chMSB;
00731   char chLSB;
00732   bool bEE_RWsuccess;
00733 
00734   // Ensure BMA180 is connected
00735   //ensure CS is high
00736   iSpiErr = 0;
00737   bEE_RWsuccess = true;
00738   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00739   //send read command
00740   chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00741   iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00742   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00743   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00744   //ensure CS is high
00745   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00746 
00747   //Verify if a BMA180 is connected on the respective ChipSelect
00748   if( (iSpiErr == 0) && (0 < (unsigned short)chCHIP_ID) && (0 < (unsigned short)chALML_VER) )
00749   {
00750     if( !bWriteEEPROM == true )
00751     {
00752       // Write X axis offset
00753       chMSB = (char) ((uiBiasX&0xFF0)>>4);
00754       chLSB = (char) ((uiBiasX&0x0F));
00755       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_X, 0, 8, chMSB, iChipSelect, handle_sub20 );
00756       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_LSB1, 4, 4, chLSB, iChipSelect, handle_sub20 );
00757 
00758       // Write Y axis offset
00759       chMSB = (char) ((uiBiasY&0xFF0)>>4);
00760       chLSB = (char) ((uiBiasY&0x0F));
00761       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_Y, 0, 8, chMSB, iChipSelect, handle_sub20 );
00762       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_LSB2, 0, 4, chLSB, iChipSelect, handle_sub20 );
00763 
00764       // Write Z axis offset
00765       chMSB = (char) ((uiBiasZ&0xFF0)>>4);
00766       chLSB = (char) ((uiBiasZ&0x0F));
00767       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_Z, 0, 8, chMSB, iChipSelect, handle_sub20 );
00768       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_OFFSET_LSB2, 4, 4, chLSB, iChipSelect, handle_sub20 );
00769 
00770       if( bEE_RWsuccess )
00771       {
00772         std::cout << "Writing to image " << std::endl;
00773         bSuccess = true;
00774       }
00775       else
00776       {
00777         bSuccess = false;
00778         std::cout << " !!!CANNOT WRITE IMAGE!!! " << std::endl;
00779       }
00780     }
00781     else
00782     {
00783       //Writing to the EEPROM is done by indirect write, i.e. setting values to image register
00784       //and then writing to the eeprom, which in fact reads the image register
00785       //Per write 16 bit from image to eeprom are written and only even addresses can used (see docu)
00786       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_EE_GAIN_Z, 0, 8, 0x00, iChipSelect, handle_sub20 );
00787       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_EE_OFFSET_LSB2, 0, 8, 0x00, iChipSelect, handle_sub20 );
00788       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_EE_OFFSET_X, 0, 8, 0x00, iChipSelect, handle_sub20 );
00789       bEE_RWsuccess &= write_bit_eeprom_sub20( bma180_cmd::chADR_EE_OFFSET_Z, 0, 8, 0x00, iChipSelect, handle_sub20 );
00790 
00791       if( bEE_RWsuccess )
00792       {
00793         std::cout << "***************EEPROM written ******************** " << std::endl;
00794         bSuccess = true;
00795       }
00796       else
00797       {
00798         bSuccess = false;
00799       }
00800     }
00801   }
00802   else
00803   {
00804     //unsuccessful operation
00805     bSuccess = false;
00806   }
00807   return bSuccess;
00808 };
00809 
00810 
00811 void Bma180::set_calibstatus( bool bCalibrate )
00812 {
00813   if( bCalibrate == true )
00814   {
00815     std::string myAnswer;
00816     std::cout << "Sensors shall be calibrated <Y/N>? ";
00817     std::getline( std::cin, myAnswer );
00818     if( (0 == myAnswer.find('Y')) || (0 == myAnswer.find('y')) )
00819     {
00820       unsigned short myCS;
00821       std::cout << "Specify ChipSelect <0..4>? ";
00822       if( std::cin >> myCS )
00823       {
00824         if( myCS < bma180_cmd::iMAXNUM_OF_SENSORS )
00825         {
00826           bExecuteCalibration = true;
00827           bCalibrationCompleted = false;
00828           iCalib_CS = myCS;
00829         }
00830         else
00831         {
00832           bExecuteCalibration = false;
00833           bCalibrationCompleted = true;
00834         }
00835       }
00836       else
00837       {
00838         bExecuteCalibration = false;
00839         bCalibrationCompleted = true;
00840         std::cin.clear();
00841       }
00842     }
00843     else
00844     {
00845       bExecuteCalibration = false;
00846       bCalibrationCompleted = true;
00847     }
00848   }
00849   else
00850   {
00851     bExecuteCalibration = false;
00852     bCalibrationCompleted = true;
00853   }
00854 };
00855 
00856 
00857 
00858 // ----------
00859 // -- MAIN --
00860 // ----------
00861 int main( int argc, char **argv )
00862 {
00863   int count = 1, i = 0;
00864 
00865   //ROS initialization and publish/subscribe setup
00866   ros::init(argc, argv, "bma180");
00867   ros::NodeHandle nh;
00868   ros::Publisher bma180_pub = nh.advertise<bma180::bma180meas>("bma180", 100);
00869 
00870   //Parameter Server values
00871   double dMaxAcc_g, dRate_Hz, dBandwidth_Hz;
00872   bool bCalibrate;
00873   std::string sSub20Serial;
00874 
00875   // Read parameters from server - if parameters are not available, the node
00876   // is initialized with default
00877   if( nh.getParam("/bma180/max_acc_g", dMaxAcc_g) == false )
00878   {
00879     dMaxAcc_g = bma180_cmd::dDEFAULT_MAXACC_g;
00880   };
00881   if( nh.getParam("/bma180/bandwidth_Hz", dBandwidth_Hz) == false )
00882   {
00883     dBandwidth_Hz = bma180_cmd::dDEFAULT_BANDWIDTH_Hz;
00884   };
00885   if( nh.getParam("/bma180/rate_Hz", dRate_Hz) == false )
00886   {
00887     dRate_Hz = bma180_cmd::dDEFAULT_RATE_Hz;
00888   };
00889   if( nh.getParam("/bma180/calibrate", bCalibrate) == false )
00890   {
00891     bCalibrate = bma180_cmd::bDEFAULT_CALIBRATE;
00892   };
00893   if( nh.getParam("/bma180/sub20serial", sSub20Serial) == false )
00894   {
00895     sSub20Serial = bma180_cmd::sSUB20SERIAL;
00896   };
00897 
00898   Bma180 bma180_attached( dMaxAcc_g, dBandwidth_Hz, bCalibrate, dRate_Hz, sSub20Serial );
00899   OneBma180Meas sOneMeas;
00900   bma180::bma180meas msg;
00901   std::list<OneBma180Meas> measurements_list;
00902 
00903 
00904   // Run sensor node
00905 
00906   //set loop rate for measurement polling
00907   ros::Rate loop_rate(dRate_Hz);
00908 
00909   while( nh.ok() )
00910   {
00911     //initiate a measurement on BMA180ies
00912     try
00913     {
00914       bma180_attached.GetMeasurements(measurements_list);
00915     }
00916     catch( std::string strType )
00917     {
00918       std::cout << " An exception occurred: " << strType << std::endl;
00919       std::exit(1);
00920     }
00921 
00922     while( !measurements_list.empty() )
00923     {
00924       sOneMeas = measurements_list.back();
00925       measurements_list.pop_back();
00926       //only publish if a successful measurement was received
00927       if( sOneMeas.bMeasAvailable == true )
00928       {
00929         msg.strIdSubDev = sOneMeas.strSerial;
00930 
00931         // collect all available chipselect values into one message
00932         for( i = 0; i < sOneMeas.iNumAccels; i++ )
00933         {
00934           msg.iChipSelect.push_back( sOneMeas.iChipSelect[i] );
00935           msg.fAccelX.push_back( sOneMeas.dAccX[i] );
00936           msg.fAccelY.push_back( sOneMeas.dAccY[i] );
00937           msg.fAccelZ.push_back( sOneMeas.dAccZ[i] );
00938         }
00939         msg.header.stamp  = sOneMeas.dtomeas;
00940         bma180_pub.publish( msg );   // publish message
00941 
00942         // clear the message values after publishing to avoid accumulation of further values on subsequent messages.
00943         msg.iChipSelect.clear();
00944         msg.fAccelX.clear();
00945         msg.fAccelY.clear();
00946         msg.fAccelZ.clear();
00947       }
00948     }
00949 
00950     ros::spinOnce();
00951     loop_rate.sleep();
00952     ++count;
00953   }
00954 }
00955 
00956 


bma180
Author(s): Lukas Marti, Nikhil Deshpande, Philip Roan (Maintained by Philip Roan)
autogenerated on Sat Dec 28 2013 16:50:12