fakeDMU.cpp

Go to the documentation of this file.

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

using namespace O_SESSAME;
using namespace std;

DMU* DMU::s_instance = NULL;

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

DMU::DMU(const string& _sInitString)
{
        Initialize(_sInitString);
}

int DMU::Initialize()
{
        // this is temporary, just used because the parsing code 
        // is not ready yet
        
        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);
        
        m_prate = 200;
        m_tempBuffSize = 100;
         
        // allocate the memory that holds the temporary read data buffer 
        m_ptrToReadData = new adat_t[m_tempBuffSize];
        
        // OK, pretend to initialize the hardware DMU.
        cout << " Initializing the DMU... " << endl;
        
        //if (dmuInit(1024, m_prate, 0))
        //{
          //  cout << " oops, 'dmuInit()' failed " << endl;         
            // some error return condition?
            //return 1;
        // }
        cout << " ok, 'dmuInit()' successfull " << endl;
        return 0;
}

int DMU::Initialize(const string& _sInitString)
{
        // code to parse info from config file
        // ... here...
        
    unsigned int find = 0; 
    int efind;
    string sParameter;
    string stringTemp; 
    //stringstream sStream;//(_sInitString.rdbuf());
    Vector tempVector3(3); // temp location vector
    Vector tempQuat4(4); // temp quaternion vector
    
    while(find < _sInitString.size())
    {
        efind = _sInitString.find(':', find);
        sParameter = _sInitString.substr(find, efind-find);
            find = efind + 1;

        if((sParameter == "Name") || (sParameter == "Number"))
        {
            efind = _sInitString.find('\n', find);
                find = efind + 1;           
        }
        else if(sParameter == "Quaternion")
        {       
            efind = _sInitString.find(',', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;     
            tempQuat4(1) = strtod(stringTemp.c_str(), (char **)NULL);
        
            efind = _sInitString.find(',', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;     
            tempQuat4(2) = strtod(stringTemp.c_str(), (char **)NULL);
        
            efind = _sInitString.find(',', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;     
            tempQuat4(3) = strtod(stringTemp.c_str(), (char **)NULL);
            
            efind = _sInitString.find('\n', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;     
            tempQuat4(4) = strtod(stringTemp.c_str(), (char **)NULL);
    
            SetSensorToBodyQuaternions(tempQuat4);
        }
        
        else if(sParameter == "Location")
        {
            efind = _sInitString.find(',', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;
            tempVector3(1) = strtod(stringTemp.c_str(), (char **)NULL);

            efind = _sInitString.find(',', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;
            tempVector3(2) = strtod(stringTemp.c_str(), (char **)NULL);

            efind = _sInitString.find('\n', find);
            stringTemp = _sInitString.substr(find, efind-find);
                find = efind + 1;
            tempVector3(3) = strtod(stringTemp.c_str(), (char **)NULL);

            SetLocation(tempVector3);
        }
    }

    // could also parse out following info from config file.
    // for now it's hard-coded
    m_prate = 200;
    m_tempBuffSize = 100;
    

    // allocate the memory that holds the temporary read data buffer 
    m_ptrToReadData = new adat_t[m_tempBuffSize];
        
    // OK, now pretend to initialize the hardware DMU.
    cout << " Initializing the DMU... " << endl;
    //if (dmuInit(1024, m_prate, 0))
    //{
//          cout << " oops, 'dmuInit()' failed " << endl;           
            // some error return condition?
//          return 1;
//    }
    cout << " ok, 'dmuInit()' successfull " << endl;
    return 0;
}

DMU* DMU::Instance()
{
        /* 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. */
        if (s_instance==NULL)
        {
                cout << " you have an instance of a new dmu " << endl;
                s_instance = new DMU();
        }
        return s_instance;
}

DMU* DMU::Instance(const string& _sInitString)
{
        /* 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. 
         
         Takes the initialization string as its argument*/
        if (s_instance==NULL) 
        {
                cout << " you have an instance of a new dmu " << endl;
                s_instance = new DMU(_sInitString);
        }
        return s_instance;
}

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_dYoBodyQuaternion
        Quaternion q_rg2_to_b;
        q_rg2_to_b = R_rg2_to_b.GetQuaternion();
        m_dmuYToBodyQuaternion = q_rg2_to_b;
        
        // convert R_rg3to_b into q_rg3_to_b
        // and set as m_dmuXToBodyQuaternion
        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;
        cout << " you gave bad channel, here's a dummy quaternion " << endl;
        return errorQuat4;
}
                        
void DMU::GetReading( int _channelID, timeval& _timeStamp, double& _rateData )
{
        cout << " you just called DMU::GetReading(...) " << endl;
        
        int numread = 0;
        //numread = dmuReadData(&m_ptrToReadData, m_tempBuffSize);
        
        static int numberOfTimesCalled = 0;
        
        // pretend dmuReadData reads no more than 10 from polled data
        for (int i=0; i<10; i++)
        {
                m_ptrToReadData[i].xrate = 0.0 + 0.1*numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].yrate = 0.0 + 0.5*numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].zrate = 0.0 + 2.0*numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].xaccel = 0.0 + numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].yaccel = 0.0 + numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].zaccel = 0.0 + numberOfTimesCalled + (float)i*0.01;
                m_ptrToReadData[i].temp = 0.0 + numberOfTimesCalled + (float)i*0.01;
                numread++;
                i++;
        }
        numberOfTimesCalled++;
        
        cout << " you read " << numread << " sets of data " << endl;
        // will give 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;
        }
        // for now just give deltat as timestamp
        //_timeStamp = m_ptrToReadData[0].deltat;
        gettimeofday(&_timeStamp, 0);
        
}

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

                        
        

---