bma180: bma180.cpp Source File

00001 /*
00002  * Copyright (c) 2010, 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 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 //\Author Lukas Marti, Bosch LLC
00031 #include <iostream>
00032 #include <string>
00033 #include <ros/ros.h>
00034 #include "bma180/bma180.h"
00035 #include <sstream>
00036 #include <ros/time.h>
00037 #include <math.h>
00038 
00039 Bma180::Bma180(double dMaxAcc_g, double dBandwidth_Hz, bool bCalibrate):
00040   bSubDeviceOpen        (false),
00041   bSubDeviceConfigured  (false)
00042 {
00043   int               iSpiErr;            //Error code fostd r Sub20 API
00044   int               iSPI_cfg_set;       //Set SPI config
00045   int               iSPI_cfg_get;       //Get SPI config
00046   int               iNumOfRanges;
00047   int               iElCount;
00048   std::stringstream ss_errmsg;
00049   sub_device        sub20dev;
00050   sub_handle        sub20handle;
00051   OneSub20Config    OneSub20Configuration;
00052 
00053   ROS_INFO("--------------------------------------");
00054   ROS_INFO("Max acceleration entered %f ", dMaxAcc_g);
00055   ROS_INFO("Sensor bandwidth entered %f ", dBandwidth_Hz);
00056   ROS_INFO("--------------------------------------");
00057 
00059   //Retrieve calibration data from user
00061   set_calibstatus(bCalibrate);
00063   // Evaluate the accelerometer range  to be selected
00065   iNumOfRanges = sizeof(bma180_cmd::chCMD_FULLSCALE_G)/sizeof(char);
00066   iElCount = 0;
00067   while ((dMaxAcc_g > bma180_cmd::dFULLRANGELOOKUP[iElCount]) && (iElCount < iNumOfRanges)) {
00068     iElCount++;
00069   };
00070   ROS_INFO("Range chosen G %f ", bma180_cmd::dFULLRANGELOOKUP[iElCount]);
00071 
00072   chMaxAccRange_selected = bma180_cmd::chCMD_FULLSCALE_G[iElCount];
00074   // Evaluate the bandwidth to be selected
00076   iNumOfRanges = sizeof(bma180_cmd::chCMD_BANDWIDTH_HZ)/sizeof(char);
00077   iElCount = 0;
00078   while ((dBandwidth_Hz > bma180_cmd::dBWLOOKUP[iElCount]) && (iElCount < iNumOfRanges)) {
00079     iElCount++;
00080   };
00081   ROS_INFO("Range chosen BW %f ", bma180_cmd::dBWLOOKUP[iElCount]);
00082 
00083   chBW_selected = bma180_cmd::chCMD_BANDWIDTH_HZ[iElCount];
00085   // Open connected Sub20 Devices
00087   sub20dev = 0;
00088   sub20dev = sub_find_devices(sub20dev);
00089   while( sub20dev != 0 ) {
00090     sub20handle = sub_open( sub20dev );
00091     // on success, sub_open returns non-zero handle
00092     if (sub20handle > 0) {
00094       // Configure Sub20 device
00096       std::cout << "---Initializing SPI Interface---" << " \n";
00097       // Read current SPI configuration
00098       iSpiErr = sub_spi_config( sub20handle, 0, &iSPI_cfg_get );
00099       std::cout << "Dev Sub config  : " << iSPI_cfg_get << " \n";
00100       //Important: The SPI interface does not work properly at higher frequencies, i.e. > 2MHz
00101       // SET: Polarity Fall, SetupSmpl,  MSB first, 2MHZ
00102       iSPI_cfg_set = SPI_ENABLE|SPI_CPOL_FALL|SPI_SETUP_SMPL|SPI_MSB_FIRST|SPI_CLK_2MHZ;
00103       //Configure SPI
00104       iSpiErr = sub_spi_config( sub20handle, iSPI_cfg_set, 0 );
00105       //Read current SPI configuration
00106       iSpiErr = sub_spi_config( sub20handle, 0, &iSPI_cfg_get );
00107       //verify if sub20 device has accepted the configuration
00108       if (iSPI_cfg_get == iSPI_cfg_set ) {
00109         std::cout<< "Configuration   : " << iSPI_cfg_set << " successfully stored \n";
00110         //Subdevice has been configured successfully
00111         OneSub20Configuration.bSubDevConfigured = true;
00112         //string needs to be cleared otherwise conversion is going wrong
00113         strSerial.clear();
00114         strSerial.resize(bma180_cmd::uSERIALNUMLENGTH);
00115         sub_get_serial_number(sub20handle, const_cast<char*>(strSerial.c_str()), strSerial.size());
00116         OneSub20Configuration.strSub20Serial    = strSerial;
00117         OneSub20Configuration.handle_subdev     = sub20handle;
00118         OneSub20Configuration.subdev                    = sub20dev;
00119         std::cout << "Device Handle   : " << OneSub20Configuration.handle_subdev << std::endl;
00120         std::cout << "Serial Number   : " << OneSub20Configuration.strSub20Serial << std::endl;
00122         // Configure Sensors on Sub20
00124         confsens_on_sub20( &OneSub20Configuration, chMaxAccRange_selected, chBW_selected );
00126         // Push element onto list of subdevices
00128         Sub20Device_list.push_back (OneSub20Configuration);
00129         std::cout << "Serial : " << OneSub20Configuration.strSub20Serial << "\n";
00130       }
00131       else {
00132         ROS_INFO("ERROR - Configuration : %d not accept by device", iSPI_cfg_set);
00133         //Subdevice could not be configured
00134         OneSub20Configuration.bSubDevConfigured = false;
00135       }
00136     }
00137     //find next device, sub_find_devices using current provides next
00138     sub20dev = sub_find_devices(sub20dev);
00139   }
00140 };
00141 
00142 Bma180::~Bma180() {
00143   std::stringstream     ss_errmsg;
00144   OneSub20Config OneSub20Configuration;
00145   //close opened subdevices
00146   while (!Sub20Device_list.empty()) {
00147     OneSub20Configuration = Sub20Device_list.back ();
00148     std::cout << "Sub device removed " << OneSub20Configuration.strSub20Serial << "\n";
00149     Sub20Device_list.pop_back ();
00150   }
00151 };
00152 
00153 void Bma180::GetMeasurements(std::list<OneBma180Meas> &list_meas ) {
00154   char                  chACC_XYZ[7]; //Containing
00155   double                dAccX, dAccY, dAccZ;
00156   double                dEstBiasX, dEstBiasY, dEstBiasZ;
00157   int                   uiBcorrX, uiBcorrY, uiBcorrZ;
00158   double                dTemp;
00159   int                   iSpiErr;
00160   OneBma180Meas         sMeas;
00161   ros::Time             dtomeas;
00162   char                  chCMD;
00163   std::stringstream         ss_errmsg;
00164   std::list<OneSub20Config>::iterator iterat;
00165   int                   iChipSelect;
00166 
00167   //verify that a subdevice is connected
00168   if (1 > (int)Sub20Device_list.size()) {
00169     throw std::string ("No SubDevice connected OR access rights to USB not given");
00170   }
00171 
00172   //Trace sub20 list
00173   for (iterat = Sub20Device_list.begin(); iterat != Sub20Device_list.end(); iterat++ ) {
00174     //verify if the sub20 device is configured (which should be the case as only configured sub20ies are pushed on list)
00175     if (iterat->bSubDevConfigured == true) {
00176       //Trace through cluster
00177       for (iChipSelect=0; iChipSelect < bma180_cmd::iMAXNUM_OF_SENSORS; iChipSelect++ ) {
00178       //verify if sensor is available on respective chipselect
00179         if (iterat->Bma180Cluster[iChipSelect].bConfigured == true) {
00180           iSpiErr = 0;
00181           //NOTE: Not executed in order to save one SPI transfer
00182           // //ensure CS is high
00183           // //iSpiErr = sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00184           // //send read command
00185           chCMD = bma180_cmd::chADR_ACCLXYZ | bma180_cmd::chFLAG_R;
00186           iSpiErr += sub_spi_transfer( iterat->handle_subdev, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00187           iSpiErr += sub_spi_transfer( iterat->handle_subdev, 0, chACC_XYZ, 7, SS_CONF(iChipSelect,SS_L) );
00188           //ensure CS is high
00189           iSpiErr += sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00190 
00191           if (iSpiErr == 0 ) {
00192             dAccX = bma180data_to_double(chACC_XYZ[1], chACC_XYZ[0], bma180_cmd::eACCEL, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00193             dAccY = bma180data_to_double(chACC_XYZ[3], chACC_XYZ[2], bma180_cmd::eACCEL, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00194             dAccZ = bma180data_to_double(chACC_XYZ[5], chACC_XYZ[4], bma180_cmd::eACCEL, iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00195             dTemp = bma180data_to_double(chACC_XYZ[6], chACC_XYZ[6], bma180_cmd::eTEMP,  iterat->Bma180Cluster[iChipSelect].dFullScaleRange );
00196             //Output the data
00197             sMeas.dAccX                         = dAccX;
00198             sMeas.dAccY                         = dAccY;
00199             sMeas.dAccZ                         = dAccZ;
00200             sMeas.dTemp                         = dTemp;
00201             sMeas.dtomeas                       = ros::Time::now();
00202             sMeas.bMeasAvailable        = true;
00203             sMeas.iChipSelect           = iChipSelect;
00204             sMeas.strSerial                     = iterat->strSub20Serial;
00205             //Push measurement onto heap
00206             list_meas.push_back (sMeas);
00207 
00209             // Execute calibration of desired
00210             // Note: For calibration procedure, only one XDIMAX box is allowed to be connected
00212             if   ( (bExecuteCalibration == true) && (2>(int)Sub20Device_list.size()) )  {
00213               if (iChipSelect==iCalib_CS) {
00214                 if  ( (calib_bma180.calibsens_completed()==false)&&(calib_bma180.calibsens_set()==false)  ) {
00215                   std::cout << " Bias read from image : " << iterat->Bma180Cluster[iChipSelect].uiBiasImageX << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageY << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageZ << std::endl;
00216                   //set the sensor to be calibrated
00217                   calib_bma180.setcalibsens(sMeas);
00218                 }
00219                 else {
00220                   calib_bma180.setdata_bma180(sMeas);
00221                 }
00222                 if ( (calib_bma180.calibsens_completed())&&(!calib_bma180.verification_active()) ) {
00224                   // Get estimated sensor biases
00226                   calib_bma180.get_estbiases(&dEstBiasX, &dEstBiasY, &dEstBiasZ);
00227                   std::cout << "estimated biases : " << dEstBiasX << " " << dEstBiasY << " " << dEstBiasZ << std::endl;
00229                   // calculate adjustment
00231                   // +0.5 for typecast to get propper rounding
00232                   // 2048: Bias has 12 bit, thus halfrange is 2^11 where as bias corr scales by range
00233                   int iBcorrX, iBcorrY, iBcorrZ;
00234                   iBcorrX =  ((short)(dEstBiasX/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5));
00235                   iBcorrY =  ((short)(dEstBiasY/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5)) ;
00236                   iBcorrZ =  ((short)((dEstBiasZ-1)/iterat->Bma180Cluster[iChipSelect].dFullScaleRange*2048+0.5)) ;
00237                   std::cout << "Bias adjustments [X Y Z] " << iBcorrX << " " << iBcorrY << " " << iBcorrZ << std::endl;
00238                   std::cout << "Image values before adjustments " << iterat->Bma180Cluster[iChipSelect].uiBiasImageX << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageY << " " << iterat->Bma180Cluster[iChipSelect].uiBiasImageZ << std::endl;
00239                   uiBcorrX = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageX - ((short)(dEstBiasX/bma180_cmd::dFULLRANGELOOKUP[6]*2048)) );
00240                   uiBcorrY = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageY - ((short)(dEstBiasY/bma180_cmd::dFULLRANGELOOKUP[6]*2048)) );
00241                   uiBcorrZ = (unsigned short) (iterat->Bma180Cluster[iChipSelect].uiBiasImageZ - ((short)((dEstBiasZ-1)/bma180_cmd::dFULLRANGELOOKUP[6]*2048)));
00242 
00243                   std::cout << "corrected biases " << uiBcorrX << " " << uiBcorrY << " " << uiBcorrZ << std::endl;
00245                   // Write to image
00247                   if (!set_biassettings(iterat->handle_subdev, iChipSelect, uiBcorrX, uiBcorrY, uiBcorrZ, false)) {
00248                     bExecuteCalibration = false;
00249                       throw std::string ("Bias corrections cannot be written to image - Calibration procedure aborted");
00250                   }
00252                   // Activate verification
00254                   if (!calib_bma180.biasverify()) {
00255                     bExecuteCalibration = false;
00256                     throw std::string ("BiasVerification cannot be executed - Calibration procedure aborted");
00257                   }
00258                 }
00259 
00260                 if ( (calib_bma180.calibsens_completed())&&(calib_bma180.verification_active())&&(calib_bma180.verification_completed()) ) {
00261                   std::cout << "-----------------------------------------" << std::endl;
00262                   std::cout << "...............VERIFY BIASES............." << std::endl;
00263                   std::cout << "-----------------------------------------" << std::endl;
00264 
00266                   // Get verified biases after image write
00268                   if (!calib_bma180.get_verifiedbiases(&dEstBiasX, &dEstBiasY, &dEstBiasZ)) {
00269                     bExecuteCalibration = false;
00270                     throw std::string ("Verified biases cannot be retrieved - Calibration procedure aborted");
00271                   };
00272 
00273                   if ( (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasX)) && (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasY)) && (bma180_cmd::dCALIB_ACCURACY > fabs(dEstBiasZ-1)) ) {
00275                     // Writing EEPROM
00277                     uiBcorrX = 0;
00278                     uiBcorrY = 0;
00279                     uiBcorrZ = 0;
00280                     std::string myAnswer;
00281                     std::cin.ignore();
00282                     std::cout << "Really write EEPROM <Y/N>? ";
00283                     std::getline( std::cin, myAnswer);
00284                     if ( (0 == myAnswer.find('Y')) || (0 == myAnswer.find('y'))   ) {
00285                       if (!set_biassettings(iterat->handle_subdev, iChipSelect, uiBcorrX, uiBcorrY, uiBcorrZ, true)) {
00286                         bExecuteCalibration = false;
00287                         throw std::string ("Biases cannot be written into EEPROM - Calibration procedure aborted");
00288                       };
00289                       ROS_INFO("++++++EEPROM HAS BEEN WRITTEN+++++++      ");
00290 
00291                     }
00292                     else {
00293                       ROS_INFO("++++++EEPROM WRITE ABORTED++++++        ");
00294                     }
00295                   }
00296                   else {
00297                     std::cout << "Error values " << fabs(dEstBiasX) << " " << fabs(dEstBiasY) << " " << fabs(dEstBiasZ-1) << std::endl;
00298                     bExecuteCalibration = false;
00299                     throw std::string ("Bias verification failed, calib values too large - Calibration procedure aborted");
00300                   }
00301                   //We are done with calibration
00302                   bExecuteCalibration = false;
00303                   std::cout << "calibration completed hehe" << std::endl;
00304                 }
00305               }
00306             }
00307             else {
00308               if ( (bExecuteCalibration == true) && (1<(int)Sub20Device_list.size()) ) {
00309                 throw  std::string ("As a pre-caution: For calibration, only ONE SubDevice must be connected");
00310               }
00311             }
00312           }
00313           else {
00314             throw std::string ("Error on SPI communication - keep on runnning though");
00315           }
00316         } //config verify
00317       } //scanning through all the chipselects
00318     }
00319     else {
00320       throw std::string ("SUB20 connected but not configured - Restart of node suggested");
00321     }
00322   } //sub20 for loop
00323 };
00324 
00325 double Bma180::bma180data_to_double(char chMSB, char chLSB, bma180_cmd::eSensorType eSensor, double dAccScale) {
00326   short         s16int;                 //2 byte int to build message
00327   double        dSensorMeas;    //Scaled sensor measurement
00328 
00329   switch (eSensor) {
00331     // Retrieve Accel Data
00333     case bma180_cmd::eACCEL:
00334       //verify if pos or neg
00335       if ( (chMSB & 0x80) == 0 ) {
00336         //positive number
00337         s16int = (short) ((((unsigned short)chMSB)&0xFF)<<6)+((((unsigned short)chLSB)&0xFC)>>2);
00338       }
00339       else {
00340         //negative number
00341         //set MSB (sign) to zero and build 2 complement unsigned; offset 8192 for 2^13
00342         s16int = (short) (((((unsigned short)chMSB)&0x7F)<<6)+((((unsigned short)chLSB)&0xFC)>>2) - 8192);
00343       }
00344       dSensorMeas = ( (double) s16int) / 8192*dAccScale;
00345       break;
00347     // Convert Temperature Data
00349     case bma180_cmd::eTEMP:
00350       //verify if pos or neg
00351       if ( (chLSB & 0x80) == 0 ) {
00352         //positive number
00353         s16int = (short) (((unsigned short)chLSB)&0xFF);
00354       }
00355       else {
00356         //negative number
00357         //set MSB (sign) to zero and build 2 complement unsigned; offset 128 for 2^7
00358         s16int = (short) ((((unsigned short)chLSB)&0x7F) - 128 );
00359       };
00360       dSensorMeas = ( (double) s16int) / 8192*2;
00361       break;
00362 
00363     default :
00364       dSensorMeas = 0;
00365   }
00366   return (dSensorMeas);
00367 };
00368 
00369 unsigned short Bma180::bma180data_to_uint(char chMSB, char chLSB, bma180_cmd::eSensorType eSensor ) {
00370   unsigned short        u16int_bias;    //2 byte int to build message
00371   switch (eSensor) {
00373     // Bias X axis data
00375     case bma180_cmd::eBIAS_X:
00376       u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0xF0)>>4);
00377       break;
00379     // Bias Y axis data
00381     case bma180_cmd::eBIAS_Y:
00382       u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0x0F));
00383       break;
00385     // Bias Z axis data
00387     case bma180_cmd::eBIAS_Z:
00388       u16int_bias = (short) ((((unsigned short)chMSB)&0xFF)<<4)+((((unsigned short)chLSB)&0xF0)>>4);
00389       break;
00390     default :
00391       u16int_bias = 0;
00392   }
00393   return (u16int_bias);
00394 };
00395 
00396 bool Bma180::read_byte_eeprom_sub20(char chADR, char* pchREGISTER, unsigned short iChipSelect, sub_handle sub_h ) {
00397   int     iSpiErr = 0;
00398   bool    bSuccess;
00399   char    chCMD;
00401   // Read register at specified adderss chADR
00403   //ensure CS is high
00404   iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00405   //send read command
00406   chCMD         = chADR | bma180_cmd::chFLAG_R;
00407   iSpiErr += sub_spi_transfer( sub_h, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00408   iSpiErr += sub_spi_transfer( sub_h, 0, pchREGISTER, 1, SS_CONF(iChipSelect,SS_L) );
00409   //ensure CS is high
00410   iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00411 
00412   if (iSpiErr == 0) {
00413     //successful operation
00414     bSuccess = true;
00415   }
00416   else {
00417     //SPI error
00418     bSuccess = false;
00419   }
00420   return (bSuccess);
00421 };
00422 
00423 bool Bma180::write_bit_eeprom_sub20(char chADR, unsigned short iLSBOfByte, unsigned short iNumOfBits, char chBitSeq, unsigned short iChipSelect, sub_handle sub_h) {
00424   char          chCMD;                  //temporary character for building commands
00425   char          chREGISTER;     //temporary read status of register
00426   char          chREGISTER_MODIFIED;
00427   int           iSpiErr;
00428   char          chMask;
00429   bool          bSuccess;
00430   bool          bEEReadSuccess;
00431 
00432   //only one byte will be handled, thus iLSBOfByte + iNumOfBits > 8 means out of range
00433   if ( iLSBOfByte + iNumOfBits <= 8 )
00434   {
00435     iSpiErr = 0;
00436     bEEReadSuccess = true;
00438     // Read register at specified address chADR
00440     chCMD       = chADR | bma180_cmd::chFLAG_R;
00441     bEEReadSuccess &= read_byte_eeprom_sub20(chCMD, &chREGISTER, iChipSelect, sub_h );
00443     // Write the specified bit into register
00445     //Build mask, i.e. zero pad the respective points where we need to have
00446     if ( iNumOfBits < 8 ) {
00447       chMask = ~(((1<<iNumOfBits)-1)<<iLSBOfByte);
00448     }
00449     else {
00450       //as function of the range check above, it can only be 8
00451       chMask = 0x00;
00452     }
00453     //write specific register
00454     chREGISTER_MODIFIED = (chREGISTER&chMask ) | ( chBitSeq << iLSBOfByte );
00455     //build up command to be written to register (note read flag is not set
00456     chCMD  = chADR;
00457     iSpiErr += sub_spi_transfer( sub_h, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00458     iSpiErr += sub_spi_transfer( sub_h, &chREGISTER_MODIFIED, 0, 1, SS_CONF(iChipSelect,SS_L) );
00459     //ensure CS is high
00460     iSpiErr += sub_spi_transfer( sub_h, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00462     // Check if register has been set (only verification)
00464     //send read command
00465     chCMD       = chADR | bma180_cmd::chFLAG_R;
00466     bEEReadSuccess &= read_byte_eeprom_sub20(chCMD, &chREGISTER, iChipSelect, sub_h);
00467 
00468     if ( (iSpiErr == 0) && (bEEReadSuccess) ) {
00469       bSuccess = true;
00470     }
00471     else {
00472       //SPI communication error
00473       bSuccess = false;
00474     }
00475   }
00476   else {
00477     //out of range
00478     bSuccess = false;
00479   }
00480   return (bSuccess);
00481 };
00482 
00483 void Bma180::confsens_on_sub20(OneSub20Config *pOneSub20Conf, char chFullscale, char chBandwidth) {
00484   int               iSpiErr;            //Error code for Sub20 API
00485   int               iChipSelect;
00486   bool              bSuccess;
00487   char              chCHIP_ID;
00488   char              chALML_VER;
00489   char              chCMD;
00490   char              chREGSTATUS;
00491   unsigned short    uiBiasX;
00492   unsigned short    uiBiasY;
00493   unsigned short    uiBiasZ;
00494   std::stringstream     ss_errmsg;
00495   sub_handle        handle_sub20;
00496   bool              bEE_RWsuccess;
00497 
00498   //retrieve handle for sub20 device
00499   handle_sub20 = pOneSub20Conf->handle_subdev;
00500   for (iChipSelect = 0; iChipSelect < bma180_cmd::iMAXNUM_OF_SENSORS; iChipSelect++) {
00501     pOneSub20Conf->Bma180Cluster[iChipSelect].iNumOfCalibMeas   = 0;
00502     iSpiErr = 0;
00503     bEE_RWsuccess = true;
00504     //ensure CS is high
00505     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00506     //send read command
00507     chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00508     iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00509     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00510     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00511     //ensure CS is high
00512     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00513 
00514     //Verify if a BMA180 is connected on the respective ChipSelect
00515     if ( (iSpiErr == 0) && (0<(unsigned short)chCHIP_ID) && (0<(unsigned short)chALML_VER)  ) {
00516       ROS_INFO("-----------------------------------------");
00517       ROS_INFO("BMA 180 - Chip Select %d", iChipSelect);
00518       ROS_INFO("CHIP ID    : %u ", (unsigned short)chCHIP_ID);
00519       ROS_INFO("ALML Ver   : %u ", (unsigned short)chALML_VER);
00520 
00521       //call method to set ee_write flag
00522       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_CTRLREG0, 4, 1, bma180_cmd::chCMD_SET_EEW, iChipSelect, handle_sub20);
00523       //issue soft reset
00524       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_SOFTRESET, 0, 8, bma180_cmd::chCMD_SOFTRESET, iChipSelect, handle_sub20);
00525       //call method to set ee_write flag
00526       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_CTRLREG0, 4, 1, bma180_cmd::chCMD_SET_EEW, iChipSelect, handle_sub20);
00527       //change maxrange of sensor
00528       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_RANGE, 1, 3, chFullscale, iChipSelect, handle_sub20);
00529       bEE_RWsuccess &= read_byte_eeprom_sub20(bma180_cmd::chADR_RANGE, &chREGSTATUS, iChipSelect, handle_sub20);
00530       //Extract range
00531       pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange = bma180_cmd::dFULLRANGELOOKUP[((unsigned short)((chREGSTATUS & 0x0E)>>1))];
00532       //change sensor bandwidth
00533       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_BWTCS, 4, 4, chBandwidth, iChipSelect, handle_sub20);
00534       bEE_RWsuccess &= read_byte_eeprom_sub20(bma180_cmd::chADR_BWTCS, &chREGSTATUS, iChipSelect, handle_sub20);
00535       //Extract range
00536       pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth = bma180_cmd::dBWLOOKUP[((unsigned short)((chREGSTATUS & 0xF0)>>4))];
00537       ROS_INFO("eeprom stored Fullrange : %f  g ", pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange);
00538       ROS_INFO("eeprom stored Bandwidth : %f  Hz", pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth);
00539       ROS_INFO("-----------------------------------------");
00540 
00541       //Read bias that are currently stored in image
00542       bEE_RWsuccess &= read_biassettings(handle_sub20, iChipSelect, &uiBiasX, &uiBiasY, &uiBiasZ);
00543       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageX = uiBiasX;
00544       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageY = uiBiasY;
00545       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageZ = uiBiasZ;
00546 
00547       if (bEE_RWsuccess) {
00548         //Set flag that sensor has been initialized
00549         pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured           = true;
00550         //successful operation
00551         bSuccess = true;
00552       }
00553       else {
00554         //Set flag that sensor has been initialized
00555         pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured           = false;
00556         //successful operation
00557         bSuccess = false;
00558       }
00559     }
00560     else {
00561       ROS_INFO("-----------------------------------------");
00562       ROS_INFO(" BMA 180 - Chip Select %d ", iChipSelect);
00563       ROS_INFO(" NO SENSOR DETECTED ");
00564       ROS_INFO("-----------------------------------------");
00565 
00566       //Set flag that sensor has been initialized
00567       pOneSub20Conf->Bma180Cluster[iChipSelect].bConfigured             = false;
00568       pOneSub20Conf->Bma180Cluster[iChipSelect].dFullScaleRange         = 0;
00569       pOneSub20Conf->Bma180Cluster[iChipSelect].dSensorBandwidth        = 0;
00570       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageX            = 0;
00571       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageY            = 0;
00572       pOneSub20Conf->Bma180Cluster[iChipSelect].uiBiasImageZ            = 0;
00573       //unsuccessful operation
00574       bSuccess = false;
00575     }
00576   }
00577 };
00578 
00579 bool Bma180::read_biassettings(sub_handle handle_sub20, int iChipSelect, unsigned short* uiBiasX, unsigned short* uiBiasY, unsigned short* uiBiasZ) {
00580   int   iSpiErr;
00581   char  chBiasXYZT[6];
00582   bool  bSuccess;
00583   char  chCMD;
00584   char  chCHIP_ID;
00585   char  chALML_VER;
00586 
00588   // Ensure BMA180 is connected
00590   //ensure CS is high
00591   iSpiErr = 0;
00592   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00593   //send read command
00594   chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00595   iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00596   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00597   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00598   //ensure CS is high
00599   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00600 
00601   //Verify if a BMA180 is connected on the respective ChipSelect
00602   if ( (iSpiErr == 0)&&(0<(unsigned short)chCHIP_ID)&&(0<(unsigned short)chALML_VER)  ) {
00603     //ensure CS is high
00604     //NOTE: Not executed in order to save one SPI transfer
00605     //iSpiErr = sub_spi_transfer( iterat->handle_subdev, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00606     //send read command
00607     chCMD = bma180_cmd::chADR_OFFSET_LSB1 | bma180_cmd::chFLAG_R;
00608     iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00609     iSpiErr += sub_spi_transfer( handle_sub20, 0, chBiasXYZT, 6, SS_CONF(iChipSelect,SS_L) );
00610     //ensure CS is high
00611     iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00612 
00613     if (iSpiErr == 0 ) {
00614       (*uiBiasX) = bma180data_to_uint(chBiasXYZT[3], chBiasXYZT[0], bma180_cmd::eBIAS_X);
00615       (*uiBiasY) = bma180data_to_uint(chBiasXYZT[4], chBiasXYZT[1], bma180_cmd::eBIAS_Y);
00616       (*uiBiasZ) = bma180data_to_uint(chBiasXYZT[5], chBiasXYZT[1], bma180_cmd::eBIAS_Z);
00617       bSuccess = true;
00618     }
00619     else {
00620       (*uiBiasX) = 0;
00621       (*uiBiasY) = 0;
00622       (*uiBiasZ) = 0;
00623       bSuccess = false;
00624     }
00625   }
00626   else {
00627      (*uiBiasX) = 0;
00628      (*uiBiasY) = 0;
00629      (*uiBiasZ) = 0;
00630      bSuccess = false;
00631   }
00632   return (bSuccess);
00633 };
00634 
00635 bool Bma180::set_biassettings(sub_handle handle_sub20, int iChipSelect, unsigned short uiBiasX, unsigned short uiBiasY, unsigned short uiBiasZ, bool bWriteEEPROM) {
00636   int   iSpiErr;                //Error code for Sub20 API
00637   bool  bSuccess;
00638   char  chCMD;
00639   char  chCHIP_ID;
00640   char  chALML_VER;
00641   char  chMSB;
00642   char  chLSB;
00643   bool  bEE_RWsuccess;
00644 
00646   // Ensure BMA180 is connected
00648   //ensure CS is high
00649   iSpiErr = 0;
00650   bEE_RWsuccess = true;
00651   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00652   //send read command
00653   chCMD   = bma180_cmd::chADR_VER|bma180_cmd::chFLAG_R;
00654   iSpiErr += sub_spi_transfer( handle_sub20, &chCMD, 0, 1, SS_CONF(iChipSelect,SS_L) );
00655   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCHIP_ID, 1, SS_CONF(iChipSelect,SS_L) );
00656   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chALML_VER, 1, SS_CONF(iChipSelect,SS_L) );
00657   //ensure CS is high
00658   iSpiErr += sub_spi_transfer( handle_sub20, 0, &chCMD, 1, SS_CONF(iChipSelect,SS_H) );
00659 
00660   //Verify if a BMA180 is connected on the respective ChipSelect
00661   if ( (iSpiErr==0) && (0<(unsigned short)chCHIP_ID) && (0<(unsigned short)chALML_VER)  ) {
00662     if ( !bWriteEEPROM == true) {
00664       // Write X axis offset
00666       chMSB = (char) ((uiBiasX&0xFF0)>>4);
00667       chLSB = (char) ((uiBiasX&0x0F));
00668       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_X, 0, 8, chMSB, iChipSelect, handle_sub20 );
00669       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_LSB1, 4, 4, chLSB, iChipSelect, handle_sub20 );
00671       // Write Y axis offset
00673       chMSB = (char) ((uiBiasY&0xFF0)>>4);
00674       chLSB = (char) ((uiBiasY&0x0F));
00675       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_Y, 0, 8, chMSB, iChipSelect, handle_sub20 );
00676       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_LSB2, 0, 4, chLSB, iChipSelect, handle_sub20 );
00678       // Write Z axis offset
00680       chMSB = (char) ((uiBiasZ&0xFF0)>>4);
00681       chLSB = (char) ((uiBiasZ&0x0F));
00682       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_Z, 0, 8, chMSB, iChipSelect, handle_sub20 );
00683       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_OFFSET_LSB2, 4, 4, chLSB, iChipSelect, handle_sub20 );
00684 
00685       if ( bEE_RWsuccess ) {
00686         std::cout << "Writing to image " << std::endl;
00687         bSuccess = true;
00688       }
00689       else {
00690         bSuccess = false;
00691         std::cout << " !!!CANNOT WRITE IMAGE!!! " << std::endl;
00692       }
00693     }
00694     else {
00695       //Writing to the EEPROM is done by indirect write, i.e. setting values to image register
00696       //and then writing to the eeprom, which in fact reads the image register
00697       //Per write 16 bit from image to eeprom are written and only even addresses can used (see docu)
00698       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_EE_GAIN_Z, 0, 8, 0x00, iChipSelect, handle_sub20 );
00699       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_EE_OFFSET_LSB2, 0, 8, 0x00, iChipSelect, handle_sub20 );
00700       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_EE_OFFSET_X, 0, 8, 0x00, iChipSelect, handle_sub20 );
00701       bEE_RWsuccess &= write_bit_eeprom_sub20(bma180_cmd::chADR_EE_OFFSET_Z, 0, 8, 0x00, iChipSelect, handle_sub20 );
00702 
00703       if ( bEE_RWsuccess ) {
00704         std::cout << "***************EEPROM written ******************** " << std::endl;
00705         bSuccess = true;
00706       }
00707       else {
00708         bSuccess = false;
00709       }
00710     }
00711   }
00712   else {
00713     //unsuccessful operation
00714     bSuccess = false;
00715   }
00716   return (bSuccess);
00717 };
00718 
00719 
00720 void Bma180::set_calibstatus(bool bCalibrate) {
00721   if (bCalibrate == true) {
00722     std::string myAnswer;
00723     std::cout << "Sensors shall be calibrated <Y/N>? ";
00724     std::getline( std::cin, myAnswer);
00725     if ( (0 == myAnswer.find('Y')) || (0 == myAnswer.find('y'))   ) {
00726       unsigned short myCS;
00727       std::cout << "Specify ChipSelect <0..4>? ";
00728       if (std::cin >> myCS) {
00729         if  (myCS < bma180_cmd::iMAXNUM_OF_SENSORS) {
00730           bExecuteCalibration = true;
00731           bCalibrationCompleted = false;
00732           iCalib_CS = myCS;
00733         }
00734         else {
00735           bExecuteCalibration = false;
00736           bCalibrationCompleted = true;
00737         }
00738       }
00739       else {
00740         bExecuteCalibration = false;
00741         bCalibrationCompleted = true;
00742         std::cin.clear();
00743       }
00744     }
00745     else {
00746       bExecuteCalibration = false;
00747       bCalibrationCompleted = true;
00748     }
00749   }
00750   else {
00751     bExecuteCalibration = false;
00752     bCalibrationCompleted = true;
00753   }
00754 };
00755 
00756 
00757 
00758 // ----------
00759 // -- MAIN --
00760 // ----------
00761 int main(int argc, char **argv) {
00762   //---------------------------------------------
00763   // Declarations
00764   //---------------------------------------------
00765   //General definitions
00766   int                           count = 1;
00767   //Publisher for BMA180 data
00768   ros::init(argc, argv, "bma180");
00769   ros::NodeHandle n;
00770   ros::Publisher bma180_pub = n.advertise<bma180::bma180meas>("bma180", 100);
00771   //Parameter Server values
00772   double        dMaxAcc_g;
00773   double        dRate_Hz;
00774   double        dBandwidth_Hz;
00775   bool          bCalibrate;
00776 
00777   //---------------------------------------------
00778   // Read initialization parameters from server
00779   //---------------------------------------------
00780   //Read parameters from server - if parameters are not available, the node
00781   //is initialized with default
00782   if (n.getParam("/drivers/bma180/max_acc_g", dMaxAcc_g)                == false ) {
00783     dMaxAcc_g = bma180_cmd::dDEFAULT_MAXACC_g;
00784   };
00785   if (n.getParam("/drivers/bma180/bandwidth_Hz", dBandwidth_Hz)         == false ) {
00786     dBandwidth_Hz = bma180_cmd::dDEFAULT_BANDWIDTH_Hz;
00787   };
00788   if (n.getParam("/drivers/bma180/rate_Hz", dRate_Hz)                   == false ) {
00789     dRate_Hz = bma180_cmd::dDEFAULT_RATE_Hz;
00790   };
00791   if (n.getParam("/drivers/bma180/calibrate", bCalibrate)               == false ) {
00792     bCalibrate = bma180_cmd::bDEFAULT_CALIBRATE;
00793   };
00794 
00795   Bma180                                bma180_attached(dMaxAcc_g, dBandwidth_Hz, bCalibrate);
00796   OneBma180Meas                 sOneMeas;
00797   bma180::bma180meas    msg;
00798   std::list<OneBma180Meas> measurements_list;
00799 
00801   // Run sensor node
00803   //set loop rate for measurement polling
00804   ros::Rate loop_rate(dRate_Hz);
00805 
00806   while (n.ok())
00807   {
00808     //initiate a measurement on BMA180ies
00809     try {
00810       bma180_attached.GetMeasurements(measurements_list);
00811     }
00812 
00813     catch ( std::string strType ) {
00814       std::cout << " An exception occurred: " << strType << std::endl;
00815       std::exit(1);
00816     }
00817 
00818     while (!measurements_list.empty()) {
00819       sOneMeas = measurements_list.back ();
00820       measurements_list.pop_back ();
00821       //only publish if a successful measurement was received
00822       if ( sOneMeas.bMeasAvailable == true ) {
00823         msg.strIdSubDev   = sOneMeas.strSerial;
00824         msg.iChipSelect   = sOneMeas.iChipSelect;
00825         msg.fAcclX        = sOneMeas.dAccX;
00826         msg.fAcclY        = sOneMeas.dAccY;
00827         msg.fAcclZ        = sOneMeas.dAccZ;
00828         msg.header.stamp  = sOneMeas.dtomeas;
00829         bma180_pub.publish(msg);
00830       }
00831     }
00832     ros::spinOnce();
00833     loop_rate.sleep();
00834     ++count;
00835   }
00836 }
00837