vos/corelibs/tracking/gpsd_position.cc

Go to the documentation of this file.

/*

    Copyright (C) 2002 Reed Hedges

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA

*/

#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>
#include <string>

#include "gpsd_position.hh"

using namespace std;

/* Constructor */
GPSDPosition::GPSDPosition(char* host, int port) throw(DeviceError) :
    socket_fd(-1), gpsd_host(host), gpsd_port(port)
{
    struct sockaddr_in address;
    if( (socket_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
        throw DeviceError(string("Error creating TCP socket: ") + strerror(errno));
    }
    struct hostent* he;
    if(!(he = gethostbyname(gpsd_host))) {
        throw DeviceError(string("Invalid address for GPS daemon: ") + gpsd_host);
    }
    memset(&address, 0, sizeof(address));
    address.sin_port=htons(gpsd_port);
    address.sin_family=AF_INET;
    address.sin_addr=*((struct in_addr *)he->h_addr);
    if( connect(socket_fd, (struct sockaddr *)&address, sizeof(address)) == -1)
{
        char port_s[16];
        sprintf(port_s, "%d", gpsd_port);
        throw DeviceError(string("Error connecting to GPS daemon ") + gpsd_host + ":" + port_s + ". " + strerror(errno));
    }
}

/* Destructor */
GPSDPosition::~GPSDPosition() {
    if ( shutdown(socket_fd, 2) < 0 ) {
        fprintf(stderr, "GPSDPosition: Warning: error closing socket. %s\n",  strerror(errno));
    }
}



/* gpsd commands are:

    Command  Reply             Meaning

    P/p      GPSD,P=%f %f      position, where %f %f are latitude and longitude
values
    D/d      GPSD,D=%s         time?? ('utc')
    A/a      GPSD,A=%f         altitude (%f is value)
    V/v      GPSD,V=%f         velocity (%f is value-- m/sec??)
    R/r      GPSD,R=%d         reset?? (where %i is 0 for ok and 1 for error??)
    S/s      GPSD,S=%d         status (0 = ok? 1 = error?)
    M/m      GPSD,M=%d         mode??

*/




/* get position */
void GPSDPosition::getPosition(double *x, double *y, double *z) throw(DeviceError) {
    char msg[128];
    int nrecvd;

    if(send(socket_fd, "P\n", 2, 0) != 2)
        throw DeviceError("Error sending command to gpsd!");
    nrecvd = recv(socket_fd, msg, 128, 0);
    sscanf(msg, "GPSD,P=%lf %lf", x, z);

    if(send(socket_fd, "A\n", 2, 0) != 2)
        throw DeviceError("Error sending command to gpsd!");
    nrecvd = recv(socket_fd, msg, 128, 0);
    sscanf(msg, "GPSD,A=%lf", y);

    *x *= xScale;
    *y *= yScale;
    *z *= zScale;
    *x += xOffset;
    *y += yOffset;
    *z += zOffset;
}

/* get status */
int GPSDPosition::getStatus() throw(DeviceError){
    int status;
    char msg[128];
    if(send(socket_fd, "S\n", 2, 0) != 2) {
        throw DeviceError("Error sending command to gpsd!");
        return -1;
    }
    recv(socket_fd, msg, 128, 0);
    sscanf(msg, "GPSD,S=%d", &status);
    return status;
}

---