PhysicalTiltSensor.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file PhysicalTiltSensor.cpp
*  \brief Implementation of the PhysicalTiltSensor class
*  \author $Author: bstreetman $
*  \version $Revision: 1.2 $
*  \date    $Date: 2004/05/04 18:33:34 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "PhysicalTiltSensor.h"
#include "utils/Time.h"
#include <fstream.h>

PhysicalTiltSensor::PhysicalTiltSensor()
{
        m_fd = -1;
}

PhysicalTiltSensor::~PhysicalTiltSensor()
{
        close(m_fd);
}

int PhysicalTiltSensor::Initialize()
{
        if (m_fd < 0) { // g_fd has NOT been set yet, then init the daisychain
                char *stringPort;
                stringPort = new char[80];
                sprintf(stringPort,"/dev/tty%s",m_Port.c_str());
        
                init_serial(stringPort,B9600,&m_fd);
                cout << stringPort << "   fd: " << m_fd << endl;
        
                char *testCommand;
                testCommand = new char[80];
                sprintf(testCommand, "R");
                say(m_fd, testCommand);
                usleep(500000);
                hear(m_fd, testCommand, 15, 0, 10000, '\r');
                cout << "Reset Returned:  " << testCommand << endl;
                
        }
                
        return 0;
}

Vector PhysicalTiltSensor::GetSensorToBodyQuaternion()
{
        Vector qwe(3);
        return qwe;
}

void PhysicalTiltSensor::GetAngles(double& pitch, double& roll, double& t)
{
        char *testCommand;
        testCommand = new char[80];
        sprintf(testCommand, "G");
        say(m_fd, testCommand);
        
        t = Now();
        usleep(1000000);
        char *Results= new char[80];
        //Results = new char[80];
        int errval;
        errval = hear(m_fd, Results, 15, 6, 100000, '\r');
        //cout << "errval:  " << errval << endl;
        
        pitch = ((int)256*(int) Results[1] + (int) Results[2])*0.00275415;//convert from hardware value to degrees
        pitch = pitch*3.14159265/180.0;//convert degress to rad
        roll = ((int)256*Results[3] + (int) Results[4])*0.00275415;
        roll = roll*3.14159265/180.0;

        if(pitch > 1.30900){
                pitch = 9999;
        }       
        if(roll > 1.30900){
                roll = 9999;
        }       
                
        return;
}

int PhysicalTiltSensor::GetFD()
{
        return m_fd;
}

void PhysicalTiltSensor::SetPort(string Port)
{
        m_Port = Port;
}

Measurement PhysicalTiltSensor::GetMeasurement()
{
        Measurement M;
        return M;
}

---