calibrate_analog.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file calibrate_analog.cpp
*  \brief This file will display to screen the calibration values needed for the analog MotionPak
*  \author $Author: cakinli $
*  \version $Revision: 1.4 $
*  \date    $Date: 2005/04/27 20:00:05 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

/* Running this program, with the appropriate config files, is one way to calibrate the analog MoPak,
including the rategyros and linear accelerometers. Each sensor has a "Offset" (a.k.a. Bias) and
"ScaleFactor" in an associated section of the Whorl config (.cfg) file.  The purpose of this program 
is to give the user these Offsets, which he or she will then manually input into the config file.  
Instructions for the calibration procedure are displayed to the user.*/

#include <Base/Whorl.h>
#include <Base/WhorlSim.h>
#include <Utils/CfgParse.h>
#include <Utils/Measurement.h>
#include <iostream>
#include <fstream>
#include <string>
using namespace std;

//
// This is a function to prompt the user at each step in the calibration
//
int promptMenu(int collectRuns)
{
        const string direction[6] = {"+Z","-Z","+X","-X","+Y","-Y"};
        int userinput = 0;

        if (collectRuns < 6)
        {
                cout << "=================================================" << endl;
                cout << " MoPak orientation: " << direction[collectRuns] << " up" << endl;
                cout << " Select number to proceed: 1 = ok,  2= abort program ";
                cin >> userinput;
                if (userinput == 1)
                {
                        // collect this data as fast as possible
                        cout << " Calibrating, please wait... " << endl;
                } else
                {
                        // end program
                        cout << endl << " Stopping program... " << endl;
                }
        } else
        {
                userinput = 1;
        }
        return userinput;
}               

int main() 
{
        cout << "=================================================" << endl;
        cout << " This program will calculate the offset and " << endl;
        cout << " scale factor for each of the 3 rate gyros and " << endl;
        cout << " linear accelerometers for analog MoPak use. " << endl << endl;
        cout << " At this time the MoPak should be placed on a " << endl;
        cout << " flat and stationary surface with the extender cable." << endl;
        cout << " Follow the prompts to re-orient the MoPak during the procedure. " << endl;

        /** Prepare for parsing */
        cfgBody *cfgDat;
        CfgParse parser;
        
        /** Parse out configuration files */
        /** note: there is nothing special about this config file other than the **/
        /** fact that the Offset values are set to 0.0 and the ScaleFactor is 1.0. **/
        cfgDat = parser.go("calibrate_analog.cfg");

        Whorl whorlOne;
        whorlOne.Initialize(cfgDat);
        double total[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; /* running total to determine avg. */
        double avg[6][6]; /* array of averages from all data collections for all sensors */
        double xlinavg[4]; /* averages of x-dir neutral linear acceleration */
        double ylinavg[4]; /* averages of y-dir neutral linear acceleration */
        double zlinavg[4]; /* averages of z-dir neutral linear acceleration */
        double linmax[3] = {0.0, 0.0, 0.0}; /* max linear acceleration */
        double linmin[3] = {0.0, 0.0, 0.0}; /* min linear acceleration */
        double linzero[3] = {0.0, 0.0, 0.0}; /* zero linear acceleration */
        double ratezero[3] = {0.0, 0.0, 0.0}; /* zero angular rate */
        double scaleFactor[3] = {0.0, 0.0, 0.0};
        double offset[3] = {0.0, 0.0, 0.0};
        double rateOffset[3] = {0.0, 0.0, 0.0};
        
        double rad2deg = 180.0/3.1415926535898; /* conversion factor */
        int collectRuns = 0; /* number of times user is prompted to collect MoPak data */
        int numSamples = 300; /* number of data samples for each calibration run */
        int promptResult = 0; /*result of user prompts */
        
        Measurement xrates[numSamples];
        Measurement yrates[numSamples];
        Measurement zrates[numSamples];
        Measurement xlinacc[numSamples];
        Measurement ylinacc[numSamples];
        Measurement zlinacc[numSamples];

        promptResult = promptMenu(collectRuns);
        while (promptResult == 1 && collectRuns < 6)
        {
                for (int i = 0; i<numSamples; i++)
                {
                        xrates[i] = whorlOne.GetRateGyro(string("dmu_rg1"))->GetMeasurement();
                        yrates[i] = whorlOne.GetRateGyro(string("dmu_rg2"))->GetMeasurement();
                        zrates[i] = whorlOne.GetRateGyro(string("dmu_rg3"))->GetMeasurement();
                        xlinacc[i] = whorlOne.GetAccelerometer(string("dmu_acc1"))->GetMeasurement();
                        ylinacc[i] = whorlOne.GetAccelerometer(string("dmu_acc2"))->GetMeasurement();
                        zlinacc[i] = whorlOne.GetAccelerometer(string("dmu_acc3"))->GetMeasurement();
                        usleep(1);
                }
                for (int cc = 0; cc<numSamples; cc++)
                {
                        total[0] += xrates[cc].GetAsDouble()*-1.0*rad2deg;
                        total[1] += yrates[cc].GetAsDouble()*-1.0*rad2deg;
                        total[2] += zrates[cc].GetAsDouble()*-1.0*rad2deg;
                        total[3] += xlinacc[cc].GetAsDouble()/-9.81;
                        total[4] += ylinacc[cc].GetAsDouble()/-9.81;
                        total[5] += zlinacc[cc].GetAsDouble()/-9.81;
                }
                for (int  ii = 0; ii < 6; ii++)
                {
                        avg[ii][collectRuns] = total[ii]/numSamples;
                        // reset total to zero after use
                        total[ii] = 0;
                }
                switch (collectRuns)
                {
                        case 0:
                                xlinavg[0] = avg[3][collectRuns];
                                ylinavg[0] = avg[4][collectRuns];
                                linmax[2] = avg[5][collectRuns];
                                break;
                        case 1:
                                xlinavg[1] = avg[3][collectRuns];
                                ylinavg[1] = avg[4][collectRuns];
                                linmin[2] = avg[5][collectRuns];
                                break;
                        case 2:
                                linmax[0] = avg[3][collectRuns];
                                ylinavg[2] = avg[4][collectRuns];
                                zlinavg[0] = avg[5][collectRuns];
                                break;
                        case 3:
                                linmin[0] = avg[3][collectRuns];
                                ylinavg[3] = avg[4][collectRuns];
                                zlinavg[1] = avg[5][collectRuns];
                                break;
                        case 4:
                                xlinavg[2] = avg[3][collectRuns];
                                linmax[1] = avg[4][collectRuns];
                                zlinavg[2] = avg[5][collectRuns];
                                break;
                        case 5:
                                xlinavg[3] = avg[3][collectRuns];
                                linmin[1] = avg[4][collectRuns];        
                                zlinavg[3] = avg[5][collectRuns];
                                break;
                        default:
                                break;
                }
                cout << "=================================================" << endl;
                cout << " X-RateGyro avg = " << avg[0][collectRuns] << " V " << endl;
                cout << " Y-RateGyro avg = " << avg[1][collectRuns] << " V " << endl;
                cout << " Z-RateGyro avg = " << avg[2][collectRuns] << " V " << endl;
                cout << " X-LinAcc avg = " << avg[3][collectRuns] << " V " << endl;
                cout << " Y-LinAcc avg = " << avg[4][collectRuns] << " V " << endl;
                cout << " Z-LinAcc avg = " << avg[5][collectRuns] << " V " << endl << endl;

                /* collect running total of the 6 rate gyro zero values for each gyro */
                ratezero[0] += avg[0][collectRuns];
                ratezero[1] += avg[1][collectRuns];
                ratezero[2] += avg[2][collectRuns];

                collectRuns++;
                promptResult = promptMenu(collectRuns);
        }
        
        if (promptResult == 1)
        {
                /* find average of zero rates */
                for(int b = 0; b<3; b++)
                {
                        rateOffset[b] = ratezero[b]/6.0;
                }

                linzero[0] = (xlinavg[0]+xlinavg[1]+xlinavg[2]+xlinavg[3])/4.0;
                linzero[1] = (ylinavg[0]+ylinavg[1]+ylinavg[2]+ylinavg[3])/4.0;
                linzero[2] = (zlinavg[0]+zlinavg[1]+zlinavg[2]+zlinavg[3])/4.0;
        
                cout << "=================================================" << endl;
                cout << " X-LinAcc zero = " << linzero[0] << " V " << endl;
                cout << " Y-LinAcc zero = " << linzero[1] << " V " << endl;
                cout << " Z-LinAcc zero = " << linzero[2] << " V " << endl << endl;
        
                cout << " X-LinAcc max, min = " << linmax[0] << ", " << linmin[0] << endl;
                cout << " Y-LinAcc max, min = " << linmax[1] << ", " << linmin[1] << endl;
                cout << " Z-LinAcc max, min = " << linmax[2] << ", " << linmin[2] << endl << endl;
        
                for (int i=0; i<3; i++)
                {
                        scaleFactor[i] = (-1.0*linmin[i] + 1.0*linmax[i])/2.0;
                        offset[i] = (linmin[i]+linzero[i]+linmax[i])/3.0;
                }
        
                cout << "=================================================" << endl;
                cout << " X-RateGyro offset = " << rateOffset[0] << endl;
                cout << " Y-RateGyro offset = " << rateOffset[1] << endl;
                cout << " Z-RateGyro offset = " << rateOffset[2] << endl;
                cout << " X-LinAcc scale factor, offset = " << scaleFactor[0] << ", " << offset[0] << endl;
                cout << " Y-LinAcc scale factor, offset = " << scaleFactor[1] << ", " << offset[1] << endl;
                cout << " Z-LinAcc scale factor, offset = " << scaleFactor[2] << ", " << offset[2] << endl;
        }
        return 0;
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: calibrate_analog.cpp,v $
*       Revision 1.4  2005/04/27 20:00:05  cakinli
*       Retry
*       
*       Revision 1.3  2004/05/27 19:04:49  shoemaker
*       Added linear accelerometer calibration routines
*       
*       Revision 1.2  2004/04/11 19:30:22  shoemaker
*       Increased number of samples taken to improve accuracy
*       
*       Revision 1.1  2004/04/08 06:06:37  shoemaker
*       Added analog MoPak calibration routines
*       
*       Revision 1.5  2004/03/29 16:43:11  shoemaker
*       Added code to work with analog DAQ card code
*       
*       
******************************************************************************/

---