realDMU.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file realDMU.h
*  \brief The implementation file of the DMU class concerning the physical (real) DMU
*  \author $Author: cakinli $
*  \version $Revision: 1.9 $
*  \date    $Date: 2005/04/26 17:20:38 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#include <iostream>
#include "DMU.h"

using namespace O_SESSAME;
using namespace std;

DMU* DMU::s_instance = NULL;

DMU::DMU(int _mode)
{
        Initialize(_mode);
}

DMU* DMU::Instance(int _mode)
{
        /* If first time being called, return a pointer 
         to an instance of a new DMU.  Otherwise return
         a pointer to an instance of a DMU which was made
         earlier.  Essentially, there can be only one DMU. 
        The _mode argument is passed through the constuctor 
        to the initialization funtion, since the DMU is initialized differently
        in analog and digital operation */
        if (s_instance==NULL)
        {
                //cout << " you have an instance of a new dmu " << endl;
                s_instance = new DMU(_mode);
        }
        return s_instance;
}

int DMU::Initialize(int _mode)
{
        // this is temporary, just used because the parsing code 
        // is not ready yet

        // hmmm.. guess this should come from config stuff????
        Vector tempVector3(3);
        Vector tempQuat4(4);

        tempVector3(1) = 0.0;
        tempVector3(2) = 0.0;
        tempVector3(3) = 0.0;

        tempQuat4(1) = 0.0;
        tempQuat4(2) = 0.0;
        tempQuat4(3) = 0.0;
        tempQuat4(4) = 1.0;

        SetLocation(tempVector3);
        SetSensorToBodyQuaternions(tempQuat4);
        
        // NOTE: done outside of _mode check to assure that data members are initialized,
        // on the off chance they will be referenced despite not being in analog or digital mode.
        // this might look alittle different with config parsing stuff.
        
        m_prate = 200;  // this variable doesn't appear to be used for anything but an assignment check?  -jls
        m_tempBuffSize = 100; // this is likely excessively large
        m_timeToler = 31250; // in usecs, 32Hz is the max rate for *serial* comm.

        m_numread = 0;
        m_firstCall = 1;

        // allocate the memory that holds the temporary read data buffer 
        m_ptrToReadData = new adat_t[m_tempBuffSize];
        
        // Now perform initialization, which depends on MoPak mode
        switch(_mode)
        {
                case 1:
                        if (dmuInit((long)1024, m_prate, 0)==-1) // the -1 from dmu.c
                        {
                                cout << " oops, 'dmuInit()' failed " << endl;       
                                // some error return condition?
                                return 1;
                        }
                        break;
                case 2:
                        // Note: there used to be code here to initialize the DAQ card,
                        // but now that is done in DAQCard::Initialize(), so it is totally separate,
                        // and this case doesn't really do anything.
                        break;
                default:
                        cout << " bad mode! " << endl;
        }
        return 0;
}

void DMU::SetLocation(const Vector& _newDeviceLocation)
{
        m_location = _newDeviceLocation;
}

Vector DMU::GetLocation()
{       
        return m_location;
}

void DMU::SetSensorToBodyQuaternions(const Vector& _newDeviceQuaternion)
{
        m_sensorToBodyQuaternion = _newDeviceQuaternion;
        
        // m_sensorToBodyQuaternion is given in config file,
        // where the frame of the DMU, F_dmu, is defined on the DMU itself,
        // the body-fixed frame, F_b, is defined elsewhere.
        
        // let each rate gyro (RG) have its own frame,
        // where the RG is defined as being located on the 1-axis,
        // and the other two axis are taken from F_dmu,
        // keeping the right-handedness
        //
        // likewise let each linear accelerometer (LAmeter) have its own frame,
        // where the LAmeter is defined as measuring positve acceleration along
        // the frame's 1-axis, and the other axes same as above.
        // (note: just "LA" by itself might be confused with linear actuator)
        
        // quaternion that describes both the 
        // orientation of "x" RG frame wrt F_dmu and the
        // orientation of "x" LAmeter frame wrt F_dmu
        Quaternion q1( 0.0, 0.0, 0.0, 1.0);
        
        // quaternion that describes both the
        // orientation of "y" RG frame wrt F_dmu and the
        // orientation of "y" LAmeter frame wrt F_dmu
        Quaternion q2( 0.5, 0.5, 0.5, -0.5);
        
        // quaternion that describes both the
        // orientation of "z" RG frame wrt F_dmu and the
        // orientation of "z" LAmeter frame wrt F_dmu
        Quaternion q3( 0.5, 0.5, 0.5, 0.5);

        // convert q1 into R_rg1_to_dmu
        Rotation R_rg1_to_dmu(q1);
        
        // convert q2 into R_rg2_to_dmu 
        Rotation R_rg2_to_dmu(q2);
        
        // convert q3 into R_rg3_to_dmu
        Rotation R_rg3_to_dmu(q3);
        
        // convert m_dmuToBodyQuaternion to R_dmu_to_b
        Quaternion q0(m_sensorToBodyQuaternion);
        Rotation R_dmu_to_b(q0);
        
        // calculate rotation from rg1 to body frame
        Rotation R_rg1_to_b;
         R_rg1_to_b = R_dmu_to_b * R_rg1_to_dmu;
        
        // calculate rotation from rg2 to body frame
        Rotation R_rg2_to_b;
        R_rg2_to_b = R_dmu_to_b * R_rg2_to_dmu;
        
        // calculate rotation from rg3 to body frame
        Rotation R_rg3_to_b;
        R_rg3_to_b = R_dmu_to_b * R_rg3_to_dmu;
        
        // convert R_rg1_to_b into q_rg1_to_b
        // and set as m_dmuXToBodyQuaternion
        Quaternion q_rg1_to_b;
        q_rg1_to_b = R_rg1_to_b.GetQuaternion();
        m_dmuXToBodyQuaternion = q_rg1_to_b;
         
        // convert R_rg2_to_b into q_rg2_to_b;
        // and set as m_dmuYToBodyQuaternion
        Quaternion q_rg2_to_b;
        q_rg2_to_b = R_rg2_to_b.GetQuaternion();
        m_dmuYToBodyQuaternion = q_rg2_to_b;
        
        // convert R_rg3_to_b into q_rg3_to_b; 
        // and set as m_dmuZToBodyQuaternion
        Quaternion q_rg3_to_b;
        q_rg3_to_b = R_rg3_to_b.GetQuaternion();
        m_dmuZToBodyQuaternion = q_rg3_to_b;
}
        
Vector DMU::GetSensorToBodyQuaternion( int id )
{
        
        if ( id == 1 | id == 4 )
        {
                // xRG or xLAmeter
                return m_dmuXToBodyQuaternion;
        }
        else if ( id == 2 | id == 5 )
        {
                // yRG or yLAmeter
                return m_dmuYToBodyQuaternion;
        }
        else if ( id == 3 | id == 6 )
        {
                // zRG or zLAmeter
                return m_dmuZToBodyQuaternion;
        }
        // no default case.
        // what about error?
        //
        // do something more elegant and logical in future
        Vector errorQuat4;
        errorQuat4(1) = 0.0;
        errorQuat4(2) = 0.0;
        errorQuat4(3) = 0.0;
        errorQuat4(4) = 1.0;
        return errorQuat4;
}

void DMU::GetAnalogReading( int _channelID, DAQCard* _mycard, timeval& _timeStamp, double& _rateData )
{
        // get current time. NOTE: Fix this time thing, as per Jana's email
        gettimeofday(&_timeStamp, NULL);

        _rateData = (double)ADscan(_mycard->m_cardData, (_channelID-1));
        
        // NOTE: this time stamp should also be fixed as per Jana's email.
        m_GetReadingTime = _timeStamp;
}
                        
void DMU::GetDigitalReading( int _channelID, timeval& _timeStamp, double& _rateData )
{
        // get current time. NOTE: Fix this time thing, as per Jana's email
        gettimeofday(&_timeStamp, NULL);

        // If this is the first call of this function, get new data
        if (m_firstCall==1) // TRUE
        {
                m_numread = dmuReadData(&m_ptrToReadData, m_tempBuffSize);
        }
        else if (m_firstCall==0) // FALSE
        {
                // not the first call, so compare times
                m_delta_t = _timeStamp.tv_usec - m_GetReadingTime.tv_usec;
                if (m_delta_t < 0) m_delta_t += 1000000;
                if (m_delta_t > m_timeToler)
                {
                        // we have exceeded our time tolerance, so get new data
                        m_numread = dmuReadData(&m_ptrToReadData, m_tempBuffSize);
                }
        }   
        // Otherwise m_ptrToReadData will still be pointing to the previous
        // data set, and since we are within the time tolerance, we'll use old data.
        // Note that dmuReadData will likely read more than one set of data for 
        // each call, so the 0 index in m_ptrToReadData[0] refers to the first set.
        switch (_channelID)
        {
                case 1:
                        _rateData = m_ptrToReadData[0].xrate;
                        break;
                case 2:
                        _rateData = m_ptrToReadData[0].yrate;
                        break;
                case 3:
                        _rateData = m_ptrToReadData[0].zrate;
                        break;
                case 4:
                        _rateData = m_ptrToReadData[0].xaccel;
                        break;
                case 5:
                        _rateData = m_ptrToReadData[0].yaccel;
                        break;
                case 6:
                        _rateData = m_ptrToReadData[0].zaccel;
                        break;
                case 7:
                        _rateData = m_ptrToReadData[0].temp;
                        break;
                default:
                        cout << " illegal channel value! " << endl;
                        // NOTE: should be something better here, like error condition
        }
        // set the time for this call to the new m_GetReadingTime
        m_GetReadingTime = _timeStamp;
        // NOTE: should add error condition that returns if numread = 0
}

DMU::~DMU()
{
        // Deconstructor
        delete[] m_ptrToReadData; /* release the memory used for the temporary read data buffer */
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: realDMU.cpp,v $
*       Revision 1.9  2005/04/26 17:20:38  cakinli
*       Updated Makefiles to build a single libdsacss.a in the src directory to alleviate linking issues with the multiple, per-directory libraries.
*       
*       Revision 1.8  2005/03/16 18:08:18  cakinli
*       *** empty log message ***
*       
*       Revision 1.7  2004/04/11 19:33:25  shoemaker
*       Fixed bugs in analog code and cleaned up random messages
*       
*       Revision 1.6  2004/04/08 06:07:08  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
*       
*       
******************************************************************************/
        

---