vos/metaobjects/property/gps_position.cc

Go to the documentation of this file.

/* $Id: gps_position.cc,v 1.19 2003/07/23 00:17:32 reed Exp $ */

/** @file gps_position.cc   Define specialized LocalProperty that gets its data from gpsd.

    Copyright (C) 2001, 2002 Reed Hedges <reed@zerohour.net>

    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 <sys/types.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>

#include <vos/corelibs/vos/vos.hh>

#include "gps_position.hh"

// initialize static members
char* GPSPosition::gpsd_host = "127.0.0.1";
int GPSPosition::gpsd_port = GPSD_DEFAULT_PORT;


/* Constructor */
GPSPosition::GPSPosition(MetaObject *super) throw(ServerCommError)
        : LocalProperty(super), Property(super), MetaObject(super), lat_scale(1), lon_scale(1), alt_scale(1), lat_off(0), lon_off(0), alt_off(0), fix_alt(false), last_lat(0), last_lon(0), last_alt(0), threshold(GPSPROP_DEFAULT_THRESHOLD)
{
   struct sockaddr_in address;
    LOG("GPSPosition", 3, "Opening connection to GPSD server at " << gpsd_host << ":" << gpsd_port);
    if( (socket_fd = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
        throw ServerCommError(string("Error creating socket: ") + sys_errlist[errno]);
    }
    struct hostent* he;
    if(!(he = gethostbyname(gpsd_host))) {
        throw ServerCommError(string("Invalid address: ") + 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 ServerCommError(string("Error connecting to server ") + gpsd_host + ":" + port_s + ". " + sys_errlist[errno]);
    }
    setPrecision(GPSPROP_DEFAULT_PRECISION);
}

/* Destructor */
GPSPosition::~GPSPosition() {
    if ( shutdown(socket_fd, 2) < 0 ) {
        LOG("GPSPosition", 0, "Warning: error closing socket. " << sys_errlist[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 GPSPosition::update() throw(ServerCommError) {
    double lat, lon, alt;
    char msg[128];
    int nrecvd;

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

    if(fix_alt) {
        alt = fixed_alt_val;
    } else {
        if(send(socket_fd, "A\n", 2, 0) != 2)
            throw ServerCommError("Error sending command to gpsd!");
        nrecvd = recv(socket_fd, msg, 128, 0);
        sscanf(msg, "GPSD,A=%lf", &alt);
    }


    if( (fabs(last_lat - lat) > threshold) || (fabs(last_lon - lon) > threshold) || (fabs(last_alt - alt) > threshold) ) {
        last_lat = lat;
        last_lon = lon;
        last_alt = alt;
        char str[128];
        sprintf(str, position_format, ((lon + lon_off) * lon_scale),
                ((alt + alt_off) * alt_scale), ((lat + lat_off) * lat_scale));
        LOG("GPS Position Property", 4, str);
        this->replace(string(str));
    }
}

/* set precision */
void GPSPosition::setPrecision(int precision) throw(range_error) {
    if(precision > 15 || precision < 0)
        throw range_error("GPS Property: Illegal precision range.");
    sprintf(position_format, "%%.%df %%.%df %%.%df", precision, precision, precision);
}

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


/* set scaling  */
void GPSPosition::setScaling(double lat, double lon, double alt) {
    lat_scale = lat;
    lon_scale = lon;
    alt_scale = alt;
}

/* set offsets */
void GPSPosition::setOffsets(double lat, double lon, double alt) {
    lat_off = lat;
    lon_off = lon;
    alt_off = alt;
}

---