cdevice.cpp

QLandkarte - use your Garmin GPS with Linux Requirements: * > Qt 4.2.x (Qt 4.1.x will not wo

/**********************************************************************************************
    Copyright (C) 2007 Oliver Eichler [e-mail]

    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 <Garmin.h>

#include <string.h>
#include <math.h>
#include <errno.h>
#include <iostream>
#include <sstream>
#include <malloc.h>


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 [e-mail]</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()
{
  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;
}