Magnetometer.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file Magnetometer.cpp
*  \brief Implementation of the Magnetometer class
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.12 $
*  \date    $Date: 2007/07/24 09:41:47 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "Magnetometer.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

/*! \brief Default Constructor
*/

Magnetometer::Magnetometer( )
{
}

/*! \brief Primary constructor
        * @param handle XML handle with current simulator as starting node
*/
Magnetometer::Magnetometer( TiXmlHandle handle ): m_magCountsBody(3) 
{
        m_magCountsSensor.initialize(3);

        char* magfn;
        magfn = (char*)malloc(50);

        buffer = (char*) malloc(100);
        magread = (char*) malloc(7);

        TiXmlHandle magHandle = handle.FirstChild("HARDWARE_PROPERTIES").FirstChild("MAGNETOMETER");

        magfn = (char *)magHandle.Child("PORT",0).Element() -> Attribute("value");

        int response; // error check for parsing
        Vector mtob(3);

        response = magHandle.Child("SENSOR_2_BODY_VECTOR", 0).Element() -> QueryDoubleAttribute("value1", &mtob(1));
        checkResponse(response);
        response = magHandle.Child("SENSOR_2_BODY_VECTOR", 0).Element() -> QueryDoubleAttribute("value2", &mtob(2));
        checkResponse(response);
        response = magHandle.Child("SENSOR_2_BODY_VECTOR", 0).Element() -> QueryDoubleAttribute("value3", &mtob(3));
        checkResponse(response);

        response = magHandle.Child("X_OFFSET", 0).Element() -> QueryDoubleAttribute("value", &m_xOff);
        checkResponse(response);
        response = magHandle.Child("Y_OFFSET", 0).Element() -> QueryDoubleAttribute("value", &m_yOff);
        checkResponse(response);
        response = magHandle.Child("MAJOR_AXIS", 0).Element() -> QueryDoubleAttribute("value", &m_majorA);
        checkResponse(response);
        response = magHandle.Child("MINOR_AXIS", 0).Element() -> QueryDoubleAttribute("value", &m_minorA);
        checkResponse(response);
        response = magHandle.Child("PHI", 0).Element() -> QueryDoubleAttribute("value", &m_phi);
        checkResponse(response);

        // convert mtob to radians
        mtob(1) = Deg2Rad(mtob(1));
        mtob(2) = Deg2Rad(mtob(2));
        mtob(3) = Deg2Rad(mtob(3));
/*
        cout << "mtob: " << mtob << endl;
        Matrix temp1(3,3);
        temp1(1,1) = cos(mtob(1));
        temp1(1,2) = sin(mtob(1));
        temp1(2,1) = -sin(mtob(1));
        temp1(2,2) = cos(mtob(1));
        temp1(3,3) = 1;
        
        cout << temp1 << endl;
        
        Matrix temp2(3,3);
        temp2(1,1) = cos(mtob(2));
        temp2(1,3) = -sin(mtob(2));
        temp2(3,1) = sin(mtob(2));
        temp2(3,3) = cos(mtob(2));
        temp2(2,2) = 1;
        cout << temp2 << endl;
        
        Matrix temp3(3,3);
        temp3(1,1) = 1;
        temp3(2,2) = cos(mtob(3));
        temp3(2,3) = sin(mtob(3));
        temp3(3,2) = -sin(mtob(3));
        temp3(3,3) = cos(mtob(3));
        
        cout << temp3 << endl;

        cout << "mult: " << temp1*temp2*temp3 << endl;
        
        cout << R3(mtob(1)) << endl;
        cout << DirectionCosineMatrix(mtob(1),mtob(2),mtob(3),321) << endl;
*/
        m_Rmb.initialize(3,3);
        m_Rmb( _(1,3), _(1,3) ) = DirectionCosineMatrix(mtob(1),mtob(2),mtob(3),321);
        
        Initialize( magfn );
}

/*! \brief Deinitialize
 */
Magnetometer::~Magnetometer( )
{

        cerr << "Deinitialize Magnetometer" << endl;

        // Set Magnetometer to factory defaults
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99D\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Factory Defaults - Response: " << endl << buffer << endl;
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << buffer << endl;

        close(m_magfd);

}


/*! \brief Magnetometer initialization function
        * @param magfn magnetometer port file name
        * @param mtob vector containing magnetometer to body 3-2-1 Euler Angles
*/
int Magnetometer::Initialize( const string& magfn ) 
{
        // Open port
        if (init_serial((char*)magfn.c_str(), B9600, &(m_magfd)) < 0 )
        {
                cerr << "ERROR: Magnetometer serial initialization failed.\n";
                exit(1);
        }               

        cerr << "\nInitialization of Magnetometer" << endl;

        DefaultConfiguration( );        

        SetBaud19200Configuration( );

        close(m_magfd); // close port

        // re-open port with 19,200 baud rate
        if (init_serial((char*)magfn.c_str(), B19200, &(m_magfd)) < 0 )
        {
                cerr << "ERROR: Magnetometer serial initialization failed.\n";
                exit(1);
        }               

        PolledDataConfiguration( );

        SetBinaryConfiguration( );
        
        SetSampleRateConfiguration( );
        
        SetResetOnConfiguration( );
        //SetResetOffConfiguration( );

        SetAveragingOnConfiguration( );
        //SetAveragingOffConfiguration( );

        ReEnterOnConfiguration( );
        //ReEnterOffConfiguration( );

        QuerryConfiguration( );
        
        return( 0 );
}


/*! \brief Set Magnetometer to factory defaults */
void Magnetometer::DefaultConfiguration( )
{
        // Set Magnetometer to factory defaults
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99D\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Factory Defaults - Response: " << endl << buffer << endl;
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << buffer << endl;
        usleep(1000000);
}


/*! \brief Set Baud Rate to 19,200 */
void Magnetometer::SetBaud19200Configuration( )
{
        // Set Baud Rate to 19,200.
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99!BR=F\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer Baud Rate to 19,200 - Response: " << endl << buffer << endl;
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << buffer << endl;
        usleep(100000);
}

/*! \brief Set Magnetometer to Polled data */
void Magnetometer::PolledDataConfiguration( )
{
        // Output polled data: Note: only implemenation
        say(m_magfd, "*99P\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to polled data mode - Response: " << buffer << endl;
        usleep(100000);
}       

/*! \brief Set Magnetometer to Continuous data */
void Magnetometer::ContinuousDataConfiguration( )
{
        say(m_magfd, "*99C\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to polled data mode - Response: " << buffer << endl;
        usleep(100000);
}       

void Magnetometer::SetBinaryConfiguration( )
{
        // Set to Binary. Note: parser code written for binary mode only.
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99B\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Binary mode - Response: " << buffer << endl;
        usleep(100000);
}

/*! \brief Set to Ascii. Note: code not setup for Ascii parsing. */
void Magnetometer::SetAsciiConfiguration( )
{
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd,"*99A\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Ascii mode - Response: " << buffer << endl;
        usleep(100000);
}

/*! \brief  Set Sample Rate ( 10, 20 , 25, 30, 40, 50, 60, 100, 123, 154 )
 *   Note: Check HMR2300 manual for Output Sample rate restrictions. Recommended < 60 sps. */
void Magnetometer::SetSampleRateConfiguration( )
{
        say(m_magfd, "*99R=154\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer sample rate - Response: " << buffer << endl;
        usleep(100000);
}
                
/*! \brief  Set/Reset ON */
void Magnetometer::SetResetOnConfiguration( )
{       
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99TN\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Set/Reset On - Response: " << buffer << endl;
        usleep(100000);
}       

/*! \brief Set/Reset Off */
void Magnetometer::SetResetOffConfiguration( )
{       
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99TF\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Set/Reset OFF - Response: " << buffer << endl;
        usleep(100000);
}

/*! \brief Set Averaging On */
void Magnetometer::SetAveragingOnConfiguration( )
{
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99VN\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Averaging On - Response: " << buffer << endl;
        usleep(100000);
}

/*! \brief Set Averaging OFF */
void Magnetometer::SetAveragingOffConfiguration( )
{
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99VF\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Averaging OFF - Response: " << buffer << endl;
        usleep(100000);
}
        
/*! \brief Re-Enter Response On */
void Magnetometer::ReEnterOnConfiguration( )
{
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99Y\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Re-Enter On - Response: " << buffer << endl;
        usleep(100000);
}       

/*! \brief Re-Enter Response OFF */
void Magnetometer::ReEnterOffConfiguration( )
{
        say(m_magfd,"*99WE\r"); // Write Enable
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        say(m_magfd, "*99N\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Set Magnetometer to Re-Enter OFF - Response: " << buffer << endl;
        usleep(100000);
}       

/*! \brief  Querry parameters */
void Magnetometer::QuerryConfiguration( )
{
        say(m_magfd, "*99Q\r");
        hear(m_magfd, buffer, 100, 0, 300000, '\r');
        cerr << "Magnetometer Setting Parameters: " << buffer << endl;
}


/*! \brief Function to return scalar measurement (not currently used)
*/
Measurement Magnetometer::GetMeasurement( )
{
        Measurement unintializedMeasurement;
        return( unintializedMeasurement );
}


/*! \brief Function to return magnetometer measurement in the form of a vector in the Sensor Frame
        * @return m_magCountsSensor vector containing magnetometer measurements in counts
*/
Vector Magnetometer::GetRawVectorMeasurement( ) 
{
        //AsciiParser( );
        
        BinaryParser( );

        return ( m_magCountsSensor );
} 

/*! \brief Parse the ascii magnetometer string into x, y, z components.
 *  The magnetometer does not seem to always return the correct format. 
 *  Recommend using binary parser.
 */
void Magnetometer::AsciiParser( )
{
        say(m_magfd, "*99P\r");                 
        
        // read data from magnetometer
        hear(m_magfd, magread, 7, 0, 300000, '\r'); // binary return string is 7 bytes
        
        stringstream os(magread);       

        char* number;
        number = (char*) malloc(5);
        
        // parse the magnetometer output string and dump into m_magCountsSensor vector
        // ASCII parser (not currently being used)
        for (int i=1;os >> buffer;i++)
        {
                if (i<4) m_magCountsSensor(i) = 1;
                if (buffer[0] == '-')
                {                               
                        os >> buffer;                           
                        if(i<4) m_magCountsSensor(i) = -1;
                }                               
                                
                if (strchr(buffer,','))
                {                               
                        strcpy(number, strtok(buffer, ","));    
                        //cerr << "Number before cat: " << number << endl;                      
                        strcat(number,strtok(NULL,"\r"));
                        //sprintf(buffer, "%s%s", number, strtok(NULL,"\r"));
                        //cerr << "Buffer after cat: " << buffer << endl;
                        strcpy(buffer, number);
                        bzero(number, sizeof(number));
                }
                
                // write if using correct index #
                if (i <4) m_magCountsSensor(i) *= atoi(buffer);                 
        }               
}



/*! \brief  Binary parser- return string has the following format
 *   return = Xh | Xl | Yh | Yl | Zh | Zl | CR
 *   where h and l are high and low bytes respectively and CR
 *   is a carriage return. The high byte is a signed int, the low unsigned.
 *   output is parsed by converting each byte to integer, left shifting
 *   the high byte 8 times and adding the two together. 
 *   The high byte is left shifted since the high and low byte comprise
 *   a single 16 bit integer and the first bit of the high byte must be multiplied
 *   by 2^8 instead of 2^0, and so on for the other bits
 *   The high byte is casted straight to an int, but for the low byte,
 *   the char must first be casted to unsigned char, then to int since that
 *   byte is unsigned
 */     
void Magnetometer::BinaryParser( )
{
        say(m_magfd, "*99P\r");                 
        
        // read data from magnetometer
        hear(m_magfd, magread, 7, 0, 300000, '\r'); // binary return string is 7 bytes
        
        m_magCountsSensor(1) = ((static_cast<int>(magread[0])) << 8) + static_cast
                                <int>(static_cast<unsigned char>(magread[1]));  

        m_magCountsSensor(2) = ((static_cast<int>(magread[2])) << 8) + static_cast
                                <int>(static_cast<unsigned char>(magread[3]));  

        m_magCountsSensor(3) = ((static_cast<int>(magread[4])) << 8) + static_cast
                                <int>(static_cast<unsigned char>(magread[5]));  
        return;
} 


/*! \brief Function to return magnetometer measurement in the form of a vector in Body frame
        * @return m_magCountsBody vector containing magnetometer measurements in counts
*/
Vector Magnetometer::GetVectorMeasurement( ) 
{
        static double xtmp, ytmp, xtmp2, ytmp2;

        GetRawVectorMeasurement( );
        
        // rotate from magnetometer to body frame       
        m_magCountsBody = m_Rmb*(m_magCountsSensor);
        
        // correct for measurement calibration
        
        xtmp = m_magCountsBody(1);
        ytmp = m_magCountsBody(2);

        // subtract offset
        xtmp = xtmp - m_xOff;
        ytmp = ytmp - m_yOff;

        // rotate ellipse
        xtmp2 = xtmp*cos(m_phi) + ytmp*sin(m_phi);      
        ytmp2 = -xtmp*sin(m_phi) + ytmp*cos(m_phi);

        // scale results for x
        xtmp2 = xtmp2*m_minorA/m_majorA;
        
        // rotate back to get final result
        m_magCountsBody(1) = xtmp2*cos(m_phi) - ytmp2*sin(m_phi);
        m_magCountsBody(2) = xtmp2*sin(m_phi) + ytmp2*cos(m_phi);

        return( m_magCountsBody );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: Magnetometer.cpp,v $
*       Revision 1.12  2007/07/24 09:41:47  jayhawk_hokie
*       Removed matrix and vector pointers.
*       
*       Revision 1.11  2007/05/30 20:37:09  bwilliam
*       Updated to include x-y calibration.
*       
*       Revision 1.10  2007/05/22 18:20:33  jayhawk_hokie
*       *** empty log message ***
*       
*       Revision 1.9  2007/05/22 16:39:35  jayhawk_hokie
*       Modified output status commands during initialization phase.
*       
*       Revision 1.8  2007/05/08 22:28:58  jayhawk_hokie
*       Added functions for configuration parameters. Changed Baud to 19,200. Still have random spike points on the Z sensor.
*       
*       Revision 1.7  2007/03/27 19:40:35  jayhawk_hokie
*       Resolved non void function warnings.
*       
*       Revision 1.6  2007/01/12 22:18:42  bwilliam
*       Modified to use XML parser.
*       
*       Revision 1.5  2006/07/20 20:37:30  bwilliam
*       Removed the transpose (oops).
*       
*       Revision 1.4  2006/07/20 20:19:51  bwilliam
*       Transposed Rmb in sensor to body rotation equation to properly rotate measurement into body frame.
*       
*       Revision 1.3  2006/07/10 21:47:00  bwilliam
*       Altered class structure and added code for magnetometer implementation.
*       
*       Revision 1.2  2003/07/04 14:13:01  simpliciter
*       Added newline at end of file.
*       
*       Revision 1.1.1.1  2003/06/06 18:44:15  simpliciter
*       Initial submission.
*       
*
******************************************************************************/

---