AshtechG12_GPS_PhysicalDevice.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file AshtechG12_GPS_PhysicalDevice.h
*  \brief Get the Satellite Position
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.1 $
*  \date    $Date: 2007/08/31 16:10:54 $
************************************************************************************************/
/*!
*
************************************************************************************************/

#include "AshtechG12_GPS_PhysicalDevice.h"


//////////////////////////////////////////
/// Commands for G12 GPS Receiver
/////////////////////////////////////////

/*! \brief Default constructor. Object will be useless until <code>Connect()</code>
 *   is called.
 */
//AshtechG12_GPS_PhysicalDevice::AshtechG12_GPS_PhysicalDevice(){}

/*! \brief Default constructor. Object will be useless until <code>Connect()</code>
 *   is called.
 */
AshtechG12_GPS_PhysicalDevice::AshtechG12_GPS_PhysicalDevice( )
    : m_clientSocket( NULL )
{
}

/*! \brief Constructs the device and calls Connect().  This method has no return
 *  value so failures in the Connect() method may not be reported.  If
 * failures must be handled, you should use the default constructor and
 * call Connect() explicitly.
 *
 * @param serverName argument is the hostname or dotted decimal IP address at
 * which GPS Receiver is to be found.  The port number is presumed to be the
 * default port.
 */
AshtechG12_GPS_PhysicalDevice::AshtechG12_GPS_PhysicalDevice( const char *serverName )
   : m_clientSocket( NULL )
{
        Connect( serverName );
}


/*! \brief Constructs the device and calls Connect().  This method has no return
 *  value so failures in the Connect() method may not be reported.  If
 * failures must be handled, you should use the default constructor and
 * call Connect() explicitly.
 *
 * @param serverName argument is the hostname or dotted decimal IP address at
 * which GPS Receiver is to be found.  The port number is presumed to be the
 * default port.
 * @param port
 */
AshtechG12_GPS_PhysicalDevice::AshtechG12_GPS_PhysicalDevice( const char *serverName, int port )
   : m_clientSocket( NULL )
{
        Connect( serverName, port );
}

/*! \brief Destroys the AshtechG12_GPS_PhysicalDevice.
 */
AshtechG12_GPS_PhysicalDevice::~AshtechG12_GPS_PhysicalDevice( )
{
        delete m_clientSocket;
}


/*! \brief Attempts to connect the physical device to GPS Receiver.
 *
 * @param serverName argument is the hostname or dotted decimal IP address at
 * which SimGEN is to be found.  The port number is presumed to be the
 * default port.
 * @return 0 on success or -1 on failure.
 */
int AshtechG12_GPS_PhysicalDevice::Connect( const char *serverName )
{
        return this->Connect( serverName, DEFAULT_PORT );
}


/*! \brief Attempts to connect the physical device to GPS Receiver.
 *
 * @param serverName argument is the hostname or dotted decimal IP address at
 * which GPS Receiver is to be found.
 * @param portNumber argument is the port to access on the server.
 * @return 0 on success or -1 on failure.
 */
int AshtechG12_GPS_PhysicalDevice::Connect( const char *serverName, int portNumber )
{
        int returnValue = -1;

        try
        {

                if ( m_clientSocket != NULL)
                {
                        delete m_clientSocket;
                }

                m_clientSocket = new ClientSocket;

                m_clientSocket -> Connect( serverName, portNumber );
        }
        catch ( SocketException& error )
        {
                std::cout << "Exception was caught: " << error.description( ) << "\n";
                return( returnValue );
        }
        returnValue = 0;
        return( returnValue );
}


int AshtechG12_GPS_PhysicalDevice::Send_i( const char *data )
{
        return this->Send_i(data, strlen(data));
}


int AshtechG12_GPS_PhysicalDevice::Send_i( const char *data, unsigned int len )
{
        int returnValue = -1;

        /* Record the time of this transmission. */
        //m_lastSendTime = Now( );
        
        string strData;

        strData.assign(data,  data + len );

        try
        {
                *m_clientSocket << strData;
        }
        catch ( SocketException& error )
        {
                std::cout << "Exception was caught: " << error.description( ) << "\n";
                return( returnValue );
        }

        returnValue = strData.length( );
        return ( returnValue );
}



/*! \brief Send Command to GPS Receiver through the GPS Server.
 *  @param sock
 *  @param buf
 *  @return 0=success, -1=failure
 *
int AshtechG12_GPS_PhysicalDevice::SendInfo(int sock, char* buf)
{
        int echolen = strlen(buf);
        int status = 0; // status of send command 
        if (send(sock, buf, echolen, 0) != echolen) {
                status = -1;
                }
        return (status);
}
*/

int AshtechG12_GPS_PhysicalDevice::Recv_i( char buf[], unsigned int len )
{

        int returnValue = -1;

        std::string buffer;

        try
        {
                *m_clientSocket >> buffer;
        }
        catch ( SocketException& error )
        {
                std::cout << "Exception was caught: " << error.description( ) << "\n";
                return( returnValue );
        }

        returnValue = buffer.length( );
        char* tmp = new char [ len-1 ];
        strcpy( tmp, buffer.c_str() );

        for( int i=0; i<returnValue+1; i++ )
        {
                buf[i] =  tmp[i];

        }

        delete [ ] tmp;
        buf[ returnValue+1 ] ='\0';

        return ( returnValue );
}


/*! \brief Receive Data from Server
 *  @param sock
 *  @param buf
 *  @parm bufsize 
 *  @return 0=success, -1=failure
 *
int AshtechG12_GPS_PhysicalDevice::RecvInfo(int sock, char *buf, int bufsize){
        int bytes = 0;
        int status = 0; // status of send command 
        if ((bytes = recv(sock, buf, bufsize-1, 0)) < 1) {
                status = -1;
                }
        buf[bufsize-1] = '\0';        // Assure null terminated string
        return (status);
        }
*/


/*! \brief Sends the $PASHQ,POS command to the G12. Queries the G12 for its present
 *   position.
 *  @param pos The position object to be populated from the G12's state
 *  @param sock
 *  @return 0=success, -1=command failure, -2=ambiguous response, -3=serial read error
 */
//int AshtechG12_GPS_PhysicalDevice::GetCurrentPosition( Position &pos, int sock )
int AshtechG12_GPS_PhysicalDevice::GetCurrentPosition( Position &pos )
{
        int BUFSIZE=200;
        int sendStatus;
        int recvStatus;
        int posStatus;
        int status=0;
        char *RESPONSE = (char*)malloc(sizeof(char)*BUFSIZE);
        /// Command to be sent to GPS Receiver
        const char *COMMAND = "$PASHQ,POS\r\n\0";
        /// Send Command to GPS Receiver Server
        if ( (sendStatus = Send_i( COMMAND ) ) <= 0) 
        {
                status = -1;
        }
        //if ((sendStatus = SendInfo(sock, COMMAND)) != 0) {
        //      status = -1;
        //        }

        /// Retrieve Command from GPS Receiver Server
        if ( ( recvStatus = Recv_i( RESPONSE, BUFSIZE ) ) <= 0 ) 
        {
                status = -2;
        }
        //if ((recvStatus = RecvInfo(sock, RESPONSE, BUFSIZE)) != 0) {
        //      status = -2;
        //      }

        /// Parse RESPONSE Character Array 
        if ( ( posStatus = pos.SetFromPOS_Message( RESPONSE ) ) != 0) 
        {
                status = -3;
        }
        
        // Free Mememory
        // free(COMMAND); // causing seg fault
        free(RESPONSE);

        return (status);
//      return pos.SetFromPOS_Message(buf);
}

/*! \brief Verifies that a message string matches its embedded checksum. The
 *   complete, including header and terminating linefeed, should be passed.
 *  @param message The complete message for which checksum is computed
 *  @return 0=success,-1=bad checksum,-2=unable to extract checksum
 */
int AshtechG12_GPS_PhysicalDevice::ValidateChecksum(const char *message)
{
        if (message == NULL)
        {
                cout << " ERROR Checksum" << endl;
                //ACE_ERROR_RETURN((LM_ERROR,
                //      "(%N:%l): Can't read checksum from NULL pointer!\n"), -2);
        }

        int msgBodyLen=strlen(message)-5;
        if (msgBodyLen < 0)
        {
                cout << " ERROR Checksum" << endl;
                //ACE_ERROR_RETURN((LM_ERROR,
                //      "(%N:%l): Can't read checksum: message too short!\n"), -2);
        }

        int checksum;
    if (sscanf(message+msgBodyLen, "*%2x\r\n", &checksum) != 1)
        {
                return -2;
        }

    // Checksum starts at message[1] because the leading '$' is omitted.
        unsigned char computedChecksum=0;
        for (int i=1; i < msgBodyLen; ++i)
        {
                computedChecksum ^= (unsigned char)message[i];
        }

        if (checksum != computedChecksum)
        {
                return -1;
        }

        return 0;
}

/**
*  Receives an acknowledge or not-acknowledge reply to a synchronous command.
* This method will throw away all input leading up to the first dollar-sign
* before parsing reply.  It assumes the port is not sending any query replies
* concurrently.
* @return 0=acknowledge, -1=not-acknowledge, -2=other(unknown) reply, -3=error while reading
*/
/*
int AshtechG12_GPS_PhysicalDevice::RecvAckNak()
{
        static char ACK[] = "$PASHR,ACK*3D\r\n";
        static char NAK[] = "$PASHR,NAK*30\r\n";
        char buf[16];
        
        int status;
        do
        {
                status = m_pPortA->recv_n(buf, 1);
                if (status == -1)
                {
                        return -3;
                }
        } while (buf[0] != '$');
        
        status = m_pPortA->recv_n(buf+1, sizeof(buf)-2);
        if (status == -1)
        {
                return -3;
        }
        buf[status+1]='\0';
        
        if (!strcmp(ACK, buf))
        {
                return 0;
        }
        else if (!strcmp(NAK, buf))
        {
                return -1;
        }
        
        return -2;
}
*/

/*
/// Set Dynamics Limits of GPS Receiver
/// For satellite suggested values (1000 m/s, 10 m/s^2)
int AshtechG12_GPS_PhysicalDevice::SendDynamicLimitsSetCmd(int vel, int accel)
{
        char buf[23];
        sprintf(buf, "$PASHS,DYN,%d,%d\r\n", vel, accel);
        return (1); // RecvAckNak();
}
*/



AshtechG12_GPS_PhysicalDevice::Position::Position()
        : m_RTCM('\0'), m_numSatellites(0), m_UTC_Timetag(0.0F), m_latitude(0.0F),
          m_latitudeSector('N'), m_longitude(0.0F), m_longitudeSector('E'),
          m_altitude(0.0F), m_groundTrack(0.0F), m_groundSpeed(0.0F),
          m_verticalVelocity(0.0F), m_PDOP(0.0F), m_HDOP(0.0F), m_VDOP(0.0F),
          m_TDOP(0.0F)
{
}


AshtechG12_GPS_PhysicalDevice::Position::Position(const Position &other)
        : m_RTCM(other.m_RTCM), m_numSatellites(other.m_numSatellites),
          m_UTC_Timetag(other.m_UTC_Timetag), m_latitude(other.m_latitude),
          m_latitudeSector(other.m_latitudeSector),
          m_longitude(other.m_longitude),
          m_longitudeSector(other.m_longitudeSector),
          m_altitude(other.m_altitude), m_groundTrack(other.m_groundTrack),
          m_groundSpeed(other.m_groundSpeed),
          m_verticalVelocity(other.m_verticalVelocity), m_PDOP(other.m_PDOP),
          m_HDOP(other.m_HDOP), m_VDOP(other.m_VDOP), m_TDOP(other.m_TDOP)
{
}


AshtechG12_GPS_PhysicalDevice::Position::Position(const char *message)
        : m_RTCM('\0'), m_numSatellites(0), m_UTC_Timetag(0.0F), m_latitude(0.0F),
          m_latitudeSector('N'), m_longitude(0.0F), m_longitudeSector('E'),
          m_altitude(0.0F), m_groundTrack(0.0F), m_groundSpeed(0.0F),
          m_verticalVelocity(0.0F), m_PDOP(0.0F), m_HDOP(0.0F), m_VDOP(0.0F),
          m_TDOP(0.0F)
{
        SetFromPOS_Message(message);
}


int AshtechG12_GPS_PhysicalDevice::Position::SetFromPOS_Message(const char *message)
{
        static const char       MSG_HEADER[]="$PASHR,POS,";
        static const int        NUM_FIELDS=17;
        static const int        MAX_FIELD_LEN=12;
        static const unsigned int MIN_MSG_LEN=sizeof(MSG_HEADER)+NUM_FIELDS+4;

        if (AshtechG12_GPS_PhysicalDevice::ValidateChecksum(message) != 0)
        {
                cerr << "ERROR Checksum" << endl;
        }

    if ( strlen( message ) < MIN_MSG_LEN )
    {
                cerr << "ERROR String Length" << endl;
    }

        if (!memcmp(MSG_HEADER, message, sizeof(MSG_HEADER)))
        {
                cerr << "ERROR Message Header" << endl;
        }
        
        char tokens[NUM_FIELDS][MAX_FIELD_LEN]; 
        const char *tokPtr=message+strlen(MSG_HEADER);

        for (int i = 0; i < NUM_FIELDS; ++i)
        {
                int tokLen=strcspn(tokPtr, ",*");
                memcpy(tokens[i], tokPtr, tokLen);
                tokens[i][tokLen] = '\0';
                tokPtr+=tokLen+1;

                if (tokPtr == NULL)
                {
                        cerr << " ERROR Nothing in tokPtr" << endl;
                }
        }

        if (*(tokPtr-1) != '*')
        {
                cerr << "ERROR: Malformed GPS Position message " << message << endl;
        }

        int tmpInt;
        float ddmmPointmmmmm;

        if (sscanf(tokens[0], "%d", &tmpInt) == 1)
        {
                //m_RTCM = (unsigned char)tmpInt;       
                m_RTCM = tmpInt;        
        }
        if (sscanf(tokens[1], "%d", &tmpInt) == 1)
        {
                m_numSatellites = tmpInt;
        }
        sscanf(tokens[2],  "%f", &m_UTC_Timetag);
        if (sscanf(tokens[3], "%f", &ddmmPointmmmmm) == 1)
        {
                float minutes=fmod(ddmmPointmmmmm, 100);
                m_latitude=(ddmmPointmmmmm-minutes)/100 + minutes/60;
        }
        sscanf(tokens[4],  "%c", &m_latitudeSector);
        if (sscanf(tokens[5], "%f", &ddmmPointmmmmm) == 1)
        {
                float minutes=fmod(ddmmPointmmmmm, 100);
                m_longitude=(ddmmPointmmmmm-minutes)/100 + minutes/60;
        }
        sscanf(tokens[6],  "%c", &m_longitudeSector);
        sscanf(tokens[7],  "%f", &m_altitude);
        // tokens[8] is reserved and should be ""
        sscanf(tokens[9],  "%f", &m_groundTrack);
        sscanf(tokens[10], "%f", &m_groundSpeed);
        sscanf(tokens[11], "%f", &m_verticalVelocity);
        sscanf(tokens[12], "%f", &m_PDOP);
        sscanf(tokens[13], "%f", &m_HDOP);
        sscanf(tokens[14], "%f", &m_VDOP);
        sscanf(tokens[15], "%f", &m_TDOP);
        // tokens[16] is firmware version code

        return 0;
}


void AshtechG12_GPS_PhysicalDevice::Position::toString(std::string &str) const
{
        std::ostringstream ss;

        ss  <<  "{ "
                "RTCM: "                        << static_cast<int>(m_RTCM) << ", "
                "Satellites: "          << static_cast<int>(m_numSatellites) << ", "
                "Timetag: "                     << m_UTC_Timetag << ", "
                "Latitude: "            << m_latitude  << " " << m_latitudeSector << ", "
                "Longitude: "           << m_longitude << " " << m_longitudeSector << ", "
                "Altitude: "            << m_altitude << ", "
                "Ground Track: "        << m_groundTrack << ", "
                "Ground Speed: "        << m_groundSpeed << ", "
                "Vert. Velocity: "      << m_verticalVelocity << ", "
                "PDOP: "                        << m_PDOP << ", "
                "HDOP: "                        << m_HDOP << ", "
                "VDOP: "                        << m_VDOP << ", "
                "TDOP: "                        << m_TDOP
                << " }";

        str=ss.str();
}


std::string AshtechG12_GPS_PhysicalDevice::Position::toString() const
{
        std::string retVal;
        toString(retVal);

        return retVal;
}


AshtechG12_GPS_PhysicalDevice::Position & 
        AshtechG12_GPS_PhysicalDevice::Position::operator =(
                const AshtechG12_GPS_PhysicalDevice::Position &other
        )
{
        if (this != &other)
        {
                memcpy(this, &other, sizeof(Position));
        }

        return *this;
}


bool AshtechG12_GPS_PhysicalDevice::Position::operator ==(
                const AshtechG12_GPS_PhysicalDevice::Position &other
        ) const
{
        if (this == &other)
        {
                return true;
        }

        return !memcmp(this, &other, sizeof(Position));
}


/*
 * Generic reply format (in regular expression syntax):
 *  $PASHR.*\*[:hex:][:hex:]\r\n
 *
 * Typical response strings:
 *  $PASHR,NAK*30
 *  $PASHR,ACK*3D
 *  $PASHR,POS,0,12,1776.12,1234.5,N,2345.6,W,30000.1,,120.3,200.5,12.2,1.2,2.3,3.4,4.5,v1.1*7b
 */

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: AshtechG12_GPS_PhysicalDevice.cpp,v $
*       Revision 1.1  2007/08/31 16:10:54  jayhawk_hokie
*       Initial Submission.
*
*
*
*
*
******************************************************************************/

---