bma180: bma180: bma180_calibrate.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 #include <math.h>
00031 #include <ros/ros.h>
00032 #include <std_msgs/String.h>
00033 #include <bma180/bma180meas.h>
00034 #include <ros/time.h>
00035 #include <bma180/bma180.h>
00036 #include <bma180/bma180_calibrate.h>
00037 
00038 bma180_calibrate::bma180_calibrate() {
00039   for (iActiveDataIndex=0; iActiveDataIndex < 2; iActiveDataIndex++) {
00040     CalibSensor[iActiveDataIndex].bCalib_complete               = false;
00041     CalibSensor[iActiveDataIndex].bCalibsensID_set              = false;
00042     CalibSensor[iActiveDataIndex].iNumOfCalibMeas               = 0;
00043   }
00044   iActiveDataIndex              = 0;    //primary is always index 0
00045   bVerification_activated = false;
00046 };
00047 
00048 
00049 bma180_calibrate::~bma180_calibrate() {
00050 };
00051 
00052 bool bma180_calibrate::setdata_bma180(OneBma180Meas OneMeas) {
00053   double        std_estbias_acc;
00054   double        measdev_accX;
00055   double        measdev_accY;
00056   double        measdev_accZ;
00057   bool  bSetdata_success = false;
00058 
00059   if (CalibSensor[iActiveDataIndex].bCalibsensID_set == true) {
00060     if  ( ( CalibSensor[iActiveDataIndex].Sensor_ID.Sub20ID == OneMeas.strSerial ) && (CalibSensor[iActiveDataIndex].Sensor_ID.iChipSelect == OneMeas.iChipSelect[0]) ) {
00061       if ( CalibSensor[iActiveDataIndex].bCalib_complete == false ) {
00062         if (CalibSensor[iActiveDataIndex].iNumOfCalibMeas == 0) {
00063           //since first data set - current measurement equals bias
00064           CalibSensor[iActiveDataIndex].dBias_AccX                      = OneMeas.dAccX[0];
00065           CalibSensor[iActiveDataIndex].dBias_AccY                      = OneMeas.dAccY[0];
00066           CalibSensor[iActiveDataIndex].dBias_AccZ                      = OneMeas.dAccZ[0];
00067           CalibSensor[iActiveDataIndex].AgeOfBias                       = OneMeas.dtomeas;
00068           CalibSensor[iActiveDataIndex].iNumOfCalibMeas++;
00069           bSetdata_success = true;
00070         }
00071         else {
00072           //calculate variance of estimated bias
00073           std_estbias_acc               = bma180_spec::std_acc_g/sqrt((double)CalibSensor[iActiveDataIndex].iNumOfCalibMeas);
00074 
00075           //calculate measurement deviation
00076           measdev_accX = fabs(OneMeas.dAccX[0] - CalibSensor[iActiveDataIndex].dBias_AccX);
00077           measdev_accY = fabs(OneMeas.dAccY[0] - CalibSensor[iActiveDataIndex].dBias_AccY);
00078           measdev_accZ = fabs(OneMeas.dAccZ[0] - CalibSensor[iActiveDataIndex].dBias_AccZ);
00079 
00080 //                                      std::cout << "Num of Meas " << CalibSensor.iNumOfCalibMeas  << "std est bias " << std_estbias_acc << " \r";
00081 
00082           //verify if an outlier is detected
00083           if  ( (measdev_accX > bma180_spec::outlier_sigma*(std_estbias_acc  + bma180_spec::std_acc_g)) ||
00084                 (measdev_accY > bma180_spec::outlier_sigma*(std_estbias_acc  + bma180_spec::std_acc_g)) ||
00085                 (measdev_accZ > bma180_spec::outlier_sigma*(std_estbias_acc  + bma180_spec::std_acc_g)) ) {
00086 
00087             // movement during calibration
00088             ROS_INFO("***DUDE - CALIBRATION - DO NOT TOUCH***");
00089             //Reset validation counter to zero
00090             CalibSensor[iActiveDataIndex].iNumOfCalibMeas = 0;
00091           }
00092           else {
00093             // Filter bias based on filterconstant tau
00094             // bias_k1 = bias_k0 + 1/Tau*(meas_k1-bias_k0), where TAU needs to be adjusted to
00095             // to the filter period as defined by the Allan Variance
00096             CalibSensor[iActiveDataIndex].dBias_AccX    = CalibSensor[iActiveDataIndex].dBias_AccX      + 1/((double)bma180_spec::calib_varreduct)*(OneMeas.dAccX[0]-CalibSensor[iActiveDataIndex].dBias_AccX);
00097             CalibSensor[iActiveDataIndex].dBias_AccY    = CalibSensor[iActiveDataIndex].dBias_AccY  + 1/((double)bma180_spec::calib_varreduct)*(OneMeas.dAccY[0]-CalibSensor[iActiveDataIndex].dBias_AccY);
00098             CalibSensor[iActiveDataIndex].dBias_AccZ    = CalibSensor[iActiveDataIndex].dBias_AccZ  + 1/((double)bma180_spec::calib_varreduct)*(OneMeas.dAccZ[0]-CalibSensor[iActiveDataIndex].dBias_AccZ);
00099 
00100             CalibSensor[iActiveDataIndex].iNumOfCalibMeas++;
00101             //set age of bias
00102             CalibSensor[iActiveDataIndex].AgeOfBias             = OneMeas.dtomeas;
00103             //verify if calibration accuracy is satisfied
00104 //                                              std::cout << " Meas Count: " << CalibSensor.iNumOfCalibMeas << " calibe reduct " <<  bma180_spec::calib_varreduct << std::endl;
00105             if ( CalibSensor[iActiveDataIndex].iNumOfCalibMeas >= bma180_spec::calib_varreduct ) {
00106               //indicate that system has been calibrated
00107               CalibSensor[iActiveDataIndex].bCalib_complete     = true;
00108               //measurement successful
00109               bSetdata_success = true;
00110               //std::cout << "Calibration completed BiasX " << CalibSensor[iActiveDataIndex].dBias_AccX << " BiasY " << CalibSensor[iActiveDataIndex].dBias_AccY << " BiasZ " << CalibSensor[iActiveDataIndex].dBias_AccZ << std::endl;
00111             }
00112           }
00113         }
00114       }
00115       else {
00116 //                      std::cout << " Calibration completed " << std::endl;
00117       }
00118     }
00119     else {
00120       //message does contain data from other sensor
00121     }
00122   }
00123   else {
00124     //Sensor is not set
00125   }
00126   return (bSetdata_success);
00127 };
00128 
00129 void bma180_calibrate::setcalibsens(OneBma180Meas OneMeas) {
00130   int iCount;
00131   for (iCount=0; iCount < 2; iCount++) {
00132     CalibSensor[iCount].Sensor_ID.iChipSelect   = OneMeas.iChipSelect[0];
00133     CalibSensor[iCount].Sensor_ID.Sub20ID               = OneMeas.strSerial;
00134     CalibSensor[iCount].bCalibsensID_set                = true;
00135     CalibSensor[iCount].bCalib_complete                 = false;
00136     CalibSensor[iCount].iNumOfCalibMeas                 = 0;
00137   }
00138 }
00139 
00140 void bma180_calibrate::clearcalibsens() {
00141   int iCount;
00142   for (iCount=0; iCount < 2; iCount++) {
00143     CalibSensor[iCount].bCalibsensID_set                = false;
00144     CalibSensor[iCount].bCalib_complete                 = false;
00145     CalibSensor[iCount].iNumOfCalibMeas                 = 0;
00146   };
00147   iActiveDataIndex = 0;
00148 };
00149 
00150 bool bma180_calibrate::calibsens_completed() {
00151   return(CalibSensor[0].bCalib_complete);
00152 };
00153 
00154 bool bma180_calibrate::verification_completed() {
00155   return(CalibSensor[1].bCalib_complete);
00156 };
00157 
00158 bool bma180_calibrate::calibsens_set() {
00159   return(CalibSensor[0].bCalibsensID_set);
00160 };
00161 
00162 bool bma180_calibrate::verification_active() {
00163   return(bVerification_activated);
00164 };
00165 
00166 bool bma180_calibrate::get_estbiases(double* dAccX, double* dAccY, double* dAccZ ) {
00167   bool bSuccess;
00168   //always provides the primary bias
00169   if ( (CalibSensor[0].bCalibsensID_set==true)&&(CalibSensor[0].bCalib_complete==true) ) {
00170     *dAccX = CalibSensor[0].dBias_AccX;
00171     *dAccY = CalibSensor[0].dBias_AccY;
00172     *dAccZ = CalibSensor[0].dBias_AccZ;
00173     bSuccess = true;
00174   }
00175   else {
00176     *dAccX = 0;
00177     *dAccY = 0;
00178     *dAccZ = 0;
00179     bSuccess = false;
00180   }
00181   return(bSuccess);
00182 };
00183 
00184 bool bma180_calibrate::biasverify(void) {
00185   bool bSuccess;
00186   if ( (CalibSensor[0].bCalibsensID_set==true)&&(CalibSensor[0].bCalib_complete==true) ) {
00187     iActiveDataIndex = 1;
00188     bVerification_activated = true;
00189     bSuccess = true;
00190   }
00191   else {
00192     bSuccess = false;
00193   }
00194   return (bSuccess);
00195 };
00196 
00197 bool bma180_calibrate::get_verifiedbiases(double* dAccX, double* dAccY, double* dAccZ ) {
00198   bool bSuccess;
00199   if ( (CalibSensor[1].bCalibsensID_set==true)&&(CalibSensor[1].bCalib_complete==true) ) {
00200     *dAccX = CalibSensor[1].dBias_AccX;
00201     *dAccY = CalibSensor[1].dBias_AccY;
00202     *dAccZ = CalibSensor[1].dBias_AccZ;
00203     bSuccess = true;
00204   }
00205   else {
00206     *dAccX = 0;
00207     *dAccY = 0;
00208     *dAccZ = 0;
00209     bSuccess = false;
00210   }
00211   return(bSuccess);
00212 };
00213