Logo Search packages:      
Sourcecode: qlandkarte version File versions  Download package

CDevice.cpp

/**********************************************************************************************
    Copyright (C) 2007 Oliver Eichler oliver.eichler@gmx.de

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

    This program 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 General Public License for more details.

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

  Garmin and MapSource are registered trademarks or trademarks of Garmin Ltd.
  or one of its subsidiaries.

**********************************************************************************************/
#include "CDevice.h"

#include "../../Platform.h"

#include <Garmin.h>

#include <string.h>
#include <math.h>
#include <errno.h>
#include <iostream>
#include <sstream>
#ifdef HAVE_MALLOC_H
#include <malloc.h>
#endif
#include <stdlib.h>

#ifndef HAVE_STRSEP
/* This function was added by JP because windows dosnt have it */

static char *
strsep(char **stringp, const char *delim)
{
    register char *s;
    register const char *spanp;
    register int c, sc;
    char *tok;

    if ((s = *stringp) == NULL)
        return (NULL);
    for (tok = s;;) {
        c = *s++;
        spanp = delim;
        do {
            if ((sc = *spanp++) == c) {
                if (c == 0)
                    s = NULL;
                else
                    s[-1] = 0;
                *stringp = s;
                return (tok);
            }
        } while (sc != 0);
    }
}
#endif

using namespace NMEATcp;
using namespace Garmin;
using namespace std;

namespace NMEATcp
{

    class CMutexLocker
    {
        public:
            CMutexLocker(pthread_mutex_t& mutex)
            : mutex(mutex) {
                pthread_mutex_lock(&mutex);
            }

            ~CMutexLocker() {
                pthread_mutex_unlock(&mutex);
            }
        private:
            pthread_mutex_t& mutex;

    };

    void GPRMC(char *GPRMCLine, Garmin::Pvt_t &pvt) {
        int i = 0;
        char *next;
        char *str;
        double temp;
        double lat=0l,lon=0l;
        double speed=0l;
        float east=0.0f,north=0.0f;
        int fix=3;
        static double s_lat, s_lon;
        static float s_east, s_north;

        str=(char*) alloca(256);
        strcpy(str,GPRMCLine);
#ifdef WIN32
        next = strtok(str, ",*");
#else
        next = strsep(&str, ",*");
#endif
        while( next ) {
#ifdef WIN32
            next = strtok(str, ",*");
#else
            next = strsep(&str, ",*");
#endif
            //      next = strsep(&str, ",*");
            i++;
            switch (i) {
                case 2:
                    if( !strcmp(next, "V") ) {
                        fix=0;
                        next=NULL;
                    }
                    break;
                case 3:
                    lat = atof(next);
                    if( lat != s_lat )
                        s_north = ((float)(lat-s_lat))*(-100);
                    s_lat = lat;

                    temp = fmod(lat, 100);
                    lat = (double)((unsigned int)(lat/100));
                    lat += temp/60;
                    // Garmin says that it should be in radians but?? -- JP
                    //lat = (lat)*(double)(M_PI/180.0);
                    break;
                case 4:
                    if( next[0] == 'S' )
                        (lat) *= -1;
                    break;
                case 5:
                    lon = atof(next);
                    if ( lon != s_lon )
                        s_east = ((float)(lon-s_lon))*(-100);
                    s_lon = lon;
                    temp = fmod(lon, 100);
                    lon = (double)((unsigned int)(lon/100));
                    lon += temp/60;
                    //
                    //lon = (lon)*(double)(M_PI/180.0);
                    break;
                case 6:
                    if( next[0]=='W')
                        (lon) *= -1;
                    break;
                case 7:
                    speed = atof(next);
                default:
                    break;
            }
        };                       //while next

        north = s_north;
        east = s_east;
        double module = sqrt(north*north+east*east);
        north/=module;
        east/=module;
        north*=0.5144444*speed;
        east*=0.5144444*speed;

        pvt.fix=fix;
        pvt.lat=lat;
        pvt.lon=lon;

        pvt.east=east;
        pvt.north=north;
        pvt.up=0.0;

        pvt.tow=84815.999999999;
        pvt.leap_scnds=14;
        pvt.wn_days=6454;
    }                            //GPRMC

    void GPGGA(char *GPGGALine, Garmin::Pvt_t &pvt) {
        int i = 0;
        char *next;
        char *str;
        double alt=0l,msl_hght=0l;
        //$GPGGA,181724.000,3436.6185,S,05834.6896,W,1,05,1.7,27.9,M,14.8,M,,0000*6B
        str=(char*) alloca(256);
        strcpy(str,GPGGALine);
#ifdef WIN32
        next = strtok(str, ",*");
#else
        next = strsep(&str, ",*");
#endif
        //      next = strsep(&str, ",*");
        while( next ) {
#ifdef WIN32
            next = strtok(str, ",*");
#else
            next = strsep(&str, ",*");
#endif
            //          next = strsep(&str, ",*");
            i++;
            switch (i) {
                case 8:
                    alt = atof(next);
                    break;
                case 10:
                    msl_hght = atof(next);
                    break;
                default:
                    break;
            }
        };                       //while next

        pvt.alt=alt;
        pvt.msl_hght=msl_hght;
    }                            //GPGGA

    void GPGSA(char *GPGSALine, Garmin::Pvt_t &pvt) {
        int i = 0;
        char *next;
        char *str;
        double epe=0l,eph=0l,epv=0l;
        str=(char*) alloca(256);
        strcpy(str,GPGSALine);
#ifdef WIN32
        next = strtok(str, ",*");
#else
        next = strsep(&str, ",*");
#endif
        //      next = strsep(&str, ",*");
        while( next ) {
#ifdef WIN32
            next = strtok(str, ",*");
#else
            next = strsep(&str, ",*");
#endif
            //          next = strsep(&str, ",*");
            i++;
            switch (i) {
                case 15:
                    epe = atof(next);
                    break;
                case 16:
                    eph = atof(next);
                    break;
                case 17:
                    epv = atof(next);
                    break;
                default:
                    break;
            }
        };                       //while next

        pvt.epe=epe;
        pvt.eph=eph;
        pvt.epv=epv;
    }                            //GPGSA

    void * rtThread(void *ptr) {
        cout << "start thread" << endl;
        char data[256];          //it should be more than enough

        CDevice * dev = (CDevice*)ptr;
        CMutexLocker lock(dev->mutex);
        try
        {
            pthread_mutex_lock(&dev->dataMutex);
            /* Take control over the device */
            dev->_acquire();

            while(dev->doRealtimeThread) {
                pthread_mutex_unlock(&dev->dataMutex);

                if(dev->serial->read(data)) {
                    if(!strncmp(data,"$GPRMC",6)) {
                        pthread_mutex_lock(&dev->dataMutex);
                        GPRMC(data,dev->PositionVelocityTime);
                        pthread_mutex_unlock(&dev->dataMutex);
                    }
                    else if(!strncmp(data,"$GPGGA",6)) {
                        pthread_mutex_lock(&dev->dataMutex);
                        GPGGA(data,dev->PositionVelocityTime);
                        pthread_mutex_unlock(&dev->dataMutex);
                    }
                    else if(!strncmp(data,"$GPGSA",6)) {
                        pthread_mutex_lock(&dev->dataMutex);
                        GPGSA(data,dev->PositionVelocityTime);
                        pthread_mutex_unlock(&dev->dataMutex);
                    }
                }

                pthread_mutex_lock(&dev->dataMutex);
            }

            dev->_release();
            pthread_mutex_unlock(&dev->dataMutex);
        }
        catch(exce_t& e) {
            pthread_mutex_trylock(&dev->dataMutex);
            dev->lasterror = "Realtime thread failed. " + e.msg;
            dev->doRealtimeThread = false;
            pthread_mutex_unlock(&dev->dataMutex);
        }
        cout << "stop thread" << endl;
        return 0;
    }

}


CDevice::CDevice()
: serial(0)
, doRealtimeThread(false)
{
    pthread_mutex_init(&dataMutex, NULL);
    PositionVelocityTime.east=0;
    PositionVelocityTime.north=0;
    PositionVelocityTime.lat=0;
    PositionVelocityTime.lon=0;
    PositionVelocityTime.alt=0;
    PositionVelocityTime.epe=0;
    PositionVelocityTime.epv=0;
    PositionVelocityTime.eph=0;
    PositionVelocityTime.fix=0;
    PositionVelocityTime.tow=0;
    PositionVelocityTime.up=0;
    PositionVelocityTime.msl_hght=0;
    PositionVelocityTime.leap_scnds=0;
    PositionVelocityTime.wn_days=0;

}


CDevice::~CDevice()
{

}


const string& CDevice::getCopyright()
{
    copyright = "<h1>QLandkarte Device Driver for NMEATcp compatible GPS</h1>"
        "<h2>Driver I/F Ver. " INTERFACE_VERSION "</h2>"
        "<p>&#169; 2007 Juan Pablo Daniel Borgna jpdborgna@e-mips.com.ar</p>"
        "<p>This driver 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 General Public License for more details. </p>";
    return copyright;
}


void CDevice::_acquire()
{
#if defined(WORDS_BIGENDIAN) || !defined(CAN_UNALIGNED)
    throw exce_t(errSync, "This device has not yet been ported to your platform.");
#endif
    serial = new CTcp(port);
    serial->open();
}


void CDevice::_setRealTimeMode(bool on)
{
    CMutexLocker lock(dataMutex);
    if(doRealtimeThread == on) return;
    doRealtimeThread = on;
    if(doRealtimeThread) {
        pthread_create(&thread,NULL,rtThread, this);
    }

}


void CDevice::_getRealTimePos(Garmin::Pvt_t& pvt)
{
    if(pthread_mutex_trylock(&mutex) != EBUSY) {
        pthread_mutex_unlock(&mutex);
        throw exce_t(errRuntime,lasterror);
    }

    CMutexLocker lock(dataMutex);
    pvt = PositionVelocityTime;

}


void CDevice::_release()
{
    if(serial == 0) return;

    serial->close();
    delete serial;
    serial = 0;
}

Generated by  Doxygen 1.6.0   Back to index