readozitrack.c

open source for car navigation in linux

#ident "@(#) readOzitrack.c         v1.0.0"
/*
 * Copyright (C) 2003 Ricardo Arroyo <[e-mail]>
 *
 * This code may be used under the terms of Version 2 of the GPL,
 * read the file COPYING for details.
 *
 * 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-1307, USA.
 ******************************************************************************/


/*****************************************************************************
  recGPS Module simulator, reads Ozi track plt files and send to requesters as
  if track points where recived from an active GPS, changing datum and units.

  It works as a daemon, receiving request thru GPSrecQ queue
  (defined in recGPS.h). After receiving a GPSREC_CON message, the module
  sents a GPSSEND_POS message with plt file data every second
 ******************************************************************************/

/* part of this code is copied from the Demonstration utility for libjeeps
   written by Alan Bleasby ([e-mail]) */



#include <stdio.h>
#include <sys/param.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <signal.h>

#include "gps.h"
#include "lc_debug.h"
#include "lc_msg_q.h"
#include "lc_sem.h"
#include "lc_syslog.h"
#include "lc_math.h"

#include "recGPS.h"
#include "rGPS_Debug.h"

#define MS2MPH(x)	(2.236936 * x)
#define MS2KPH(x)	(x * 3.6)
#define MS2KNOT(x)	(1.943845 * x)
#define KNOT2MS(x)	(x / 1.943845)

#define M2FEET(x)	(3.28084 * x)
#define FEET2M(x)	(0.3048 * x)
#define M2NMILE(x)	(0.0005399568 * x)


#define PREV_DATA_INVALID	999



int iDebug; /* global variable to store debuging flags */


static int iQid; /* msg queue id, used by main and vMsgLoop */
static int iSemId; /* Semaphore id, used by main and vMsgLoop */

static RGPS_con *pConList; /* array of connected programs data */
static int iConNo = 0; /* Number of active connections */

static pthread_t thtLoopThread; /* structure for store thread info */

static FILE *fdPLTfile = NULL; /* file descriptor for PLT file */

static int32 i32GPSDatum; /* Datum of PLT file */


/* From Demonstration utility for libjeeps
   @author: Copyright (C) Alan Bleasby ([e-mail]) */
/*----------------------------------------------------------------------------*/
/*  FUNCTION :   vGetInitData(int *piTO, key_t *pktQueue, key_t *pktSem)      */
/*                                                                            */
/*          .-   reads the initialization data                                */
/*                                                                            */
/* INPUT PARAMETERS        iTO         Timeout in seconds between info        */
/*                                                                            */
/* OUTPUT PARAMETERS       none                                               */
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static void vGetInitData(int *piTO, key_t *pktQueue, key_t *pktSem)
{
    char sGPSdir[MAXPATHLEN];
    struct stat buf;
    FILE *inf;
    char line[MAXPATHLEN];

    char *p;
    char sComms[MAXPATHLEN];

    *piTO = 1; /* defalut TO, 1 sec. */
    *pktQueue = GPS_REC_Q; /* default GPS reception module queue key */
    *pktSem = GPS_REC_SEM; /* defalult GPS reception module semaphore key */

    p = getenv("LINUXCAR_DIR");
    if(p)
    {
	(void) strcpy(sGPSdir,p);
	(void) strcat(sGPSdir,"/");
    }
    else
	(void) strcpy(sGPSdir,"");
    (void) strcat(sGPSdir,".GPSconf");
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"vGetInitData: ConfFile = \"%s\"\n",sGPSdir);
    }

    if(!stat(sGPSdir,&buf))
    {
	if(buf.st_mode | S_IRUSR)
	{
	    if((inf=fopen(sGPSdir,"r")))
	    {
		while(fgets(line,MAXPATHLEN,inf))
		{
		    if (iDebug & DEP_GPSREC)
		    {
			fprintf(stderr,"vGetInitData: line = %s\n",line);
		    }
		    if(*line=='#' || *line=='\n') continue;
		    line[strlen(line)-1]='\0';

		    if(!strncmp(line,"TIMEOUT",7))
		    {
			sscanf(&line[7],"%d",piTO);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"vGetInitData: TO = %d\n",*piTO);
		    	}
		    }
		    if(!strncmp(line,"REC_QUEUE",9))
		    {
			sscanf(&line[9],"%d",pktQueue);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"vGetInitData: Queue = %d\n",*pktQueue);
		    	}
		    }
		    if(!strncmp(line,"REC_SEM",7))
		    {
			sscanf(&line[7],"%d",pktSem);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"vGetInitData: Semaphore = %d\n",*pktSem);
		    	}
		    }
		}
	    }
	}
    }

    return;
}


/*----------------------------------------------------------------------------*/
/*  FUNCTION :   void vRemoveConn(key_t ktQueue)                              */
/*                                                                            */
/*          .-   Deletes a connection from the pConList active                */
/*               connections list                                             */
/*                                                                            */
/* INPUT PARAMETERS        ktQueue    identifier of remote connection queue   */
/*                                                                            */
/* OUTPUT PARAMETERS       none                                               */
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static void vRemoveConn( key_t ktQueue)
{
int i;
RGPS_con *p;

    for (i=0; i < iConNo; i++)
    { /* loops for each active connection */
        if(pConList[i].ktQueue == ktQueue)
	{ /* found connection to be deleted */

	    if (iDebug & DEP_GPSREC)
	    {
	        fprintf(stderr,"vRemoveConn: Connection to delete: i=%d\n",i);
	    }

	    /* Closes queue of connection to delete */
//	    if (iDelQmsg(pConList[i].iQid) < 0 )
//            {
//                vLC_syslog(LCLOG_ERR_MSGQDEL, "recGPS/vRemoveConn", pConList[i].iQid);
//            }

	    /* Create new storage area for conection data -if remains any active connection- */
	    if ((iConNo -1) > 0)
	    {
		p = malloc (sizeof(RGPS_con)*(iConNo-1));
		if (p == NULL)
		{
			vLC_syslog(LCLOG_ERR_MALLOC,"recGPS/vRemoveConn");
			exit(-1);
		}

		/* move connections before one to delete, if not first */
		if ( i > 0)
		{
			memcpy(p, pConList, sizeof(RGPS_con) * i);
		}
		/* move connectios after one to delete, if no last */
		if ( i != iConNo-1 )
		{
			if (iDebug & DEP_GPSREC)
			{
			    fprintf(stderr,"vRemoveConn: sizeof(RGPS_con)=%d\n",sizeof(RGPS_con));
			    fprintf(stderr,"vRemoveConn: i=%d, p=%p, p[i]=%p\n",i, p, &p[i]);
			    fprintf(stderr,"vRemoveConn: iConNo=%d, pConList=%p, pConList[i+1]=%p\n",iConNo, pConList, &pConList[i+1]);
			}
			memcpy(&p[i], &pConList[i+1], sizeof(RGPS_con) * (iConNo-i-1));
			if (iDebug & DEP_GPSREC)
			{
			    fprintf(stderr,"vRemoveConn: pConList copied to p\n");
			}
		}
	    }
	    else
	    { /* only one connection active, after delete it, no active connections will left */
	        p = NULL;
	    }

	    free(pConList);
	    pConList = p;
	    iConNo--;

	    if (iDebug & DEP_GPSREC)
	    {
	        fprintf(stderr,"vRemoveConn: iConNo=%d, pConList=%p\n",iConNo, pConList);
	    }

            break;
	}
    }

}


/*----------------------------------------------------------------------------*/
/*  FUNCTION :   vMsgLoop (void *pNotUsed)                                    */
/*                                                                            */
/*          .-   Loops waiting for MSG queue input.                           */
/*               messages received will add or delete queues (conections)     */
/*               where position messages will be sent                         */
/*                                                                            */
/* INPUT PARAMETERS        iNotUsed    Mandatory for pthread use, not used    */
/*                                                                            */
/* OUTPUT PARAMETERS       none                                               */
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static void vMsgLoop (int *iNotUsed)
{
    RGPS_ctlMsg *pMsg;
    
    void *p;
    int i;
    
    char stLine[256];

    for(;;)
    {
        if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"vMsgLoop: Qid=%d, SemId=%d\n",iQid, iSemId);
        }
        pMsg = (RGPS_ctlMsg *)pRecQueue(iQid);
	if (pMsg == NULL)
	{
	    vLC_syslog(LCLOG_ERR_MSGQRECEIVE, "recGPS/vMsgLoop", iQid);
	    exit (-1);
	}

	/* message received */
        if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"vMsgLoop, message received: Type=%ld, Id=%d\n",pMsg->mtype, pMsg->iRGPSid);
        }
	switch (pMsg->iRGPSid)
	{
	    case GPSREC_CON: /* connection message received */
                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr,"vMsgLoop, connect message received: Queue=%d, Datum=%d, Units=%d, PosFormat=%d\n",
		                   pMsg->RGPSctl.stcRGPS_con.ktQueue,
				   pMsg->RGPSctl.stcRGPS_con.iDatum,
				   pMsg->RGPSctl.stcRGPS_con.btUnits,
				   pMsg->RGPSctl.stcRGPS_con.btPosFormat);
                }

		/* start of exclusive area */
		if (iLockSem(iSemId) != 0)
		{
		    vLC_syslog(LCLOG_ERR_SEMLK, "recGPS/vMsgLoop", iSemId);
		    exit(-1);
		}

		/* add a new connection and store conection data */
		p = malloc (sizeof(RGPS_con)*(iConNo+1));
		if (p == NULL)
		{
                    vLC_syslog(LCLOG_ERR_MALLOC,"recGPS/vMsgLoop");
		    exit(-1);
		}
		if (iConNo > 0)
		{ /* move current connection list, if it not empty */
		    memcpy(p, pConList, sizeof(RGPS_con) * iConNo); /* move current memory list */
		    free(pConList);
		    pConList = NULL;
		}
		pConList = p;
		/* move new connection data to last entry in list */
		memcpy(&pConList[iConNo], &(pMsg->RGPSctl.stcRGPS_con), sizeof(RGPS_con));

		/* Opens queue where data should be sent */
		pConList[iConNo].iQid = iOpenQmsg(pConList[iConNo].ktQueue);
                if (pConList[iConNo].iQid < 0)
                {
                    vLC_syslog(LCLOG_ERR_MSGQOPEN, "recGPS/vMsgLoop", pConList[iConNo].ktQueue);
	            exit(-1);
                }

		/* Send Connection ACK message */
		pMsg->iRGPSid = GPSREC_ACK;
		if (iSend2Queue(pConList[iConNo].iQid, pMsg, sizeof(RGPS_ctlMsg)) < 0)
		{ /* this free the memory allocated in pMsg */
			vLC_syslog(LCLOG_ERR_MSGQSEND, "recGPS/vMsgLoop", pConList[iConNo].iQid);
		}


		/* updates no of open connections */
		iConNo++;

                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr, "vMsgLoop, data in connection list\n");
		    for (i=0; i < iConNo; i++)
		    {
		        fprintf(stderr,">>  Datum=%d, Units=%d, PosFormat=%d, Queue=%d, Qid=%d\n",
				   pConList[i].iDatum,
				   pConList[i].btUnits,
				   pConList[i].btPosFormat,
		                   pConList[i].ktQueue,
		                   pConList[i].iQid);
                    }
		}

		/* end of exclusive area */
		if (iUnlockSem(iSemId) != 0)
		{
		    vLC_syslog(LCLOG_ERR_SEMULK, "recGPS/vMsgLoop", iSemId);
		    exit(-1);
		}

	        break;

	    case GPSREC_DISC: /* disconnect message received */
                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr,"vMsgLoop, disconnect message received: Queue=%d\n",
		                   pMsg->RGPSctl.stcRGPS_disc.ktQueue);
	        }
		vRemoveConn(pMsg->RGPSctl.stcRGPS_disc.ktQueue);

		free(pMsg);
		pMsg = NULL;
	        break;

	    case GPSREC_PLTFILE: /* new PLT file message received */
                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr,"vMsgLoop, New PLT message received: file=%s\n",
		                   pMsg->RGPSctl.stcRGPS_PLTfile.stFileName);
	        }
		if (fdPLTfile != NULL)
		{
		    fclose(fdPLTfile);
		}
		fdPLTfile = fopen(pMsg->RGPSctl.stcRGPS_PLTfile.stFileName, "r");
		fgets(stLine, 256, fdPLTfile);
		stLine[strlen(stLine)-2] = '\0'; /* eliminates <CR> <LF> characters */
                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr,"vMsgLoop, New PLT message received: first line in file=[%s]\n", stLine);
	        }
		if (strcmp(stLine, "OziExplorer Track Point File Version 2.1") != 0)
		{ /* file is not a Oziexplorer track file */
		    fclose(fdPLTfile);
		    fdPLTfile = NULL;
		}
		else
		{
		    fgets(stLine, 256, fdPLTfile); /* WGS 84, line */
		    stLine[strlen(stLine)-2] = '\0'; /* eliminates <CR> <LF> characters */
		    /* Get Datum of GPS */
                    i32GPSDatum = GPS_Math_Get_Datum_Number(stLine);
                    if (iDebug & DEP_GPSREC)
                    {
                        fprintf(stderr,"vMsgLoop: PLT file Datum:%s (%d)\n",stLine, i32GPSDatum);
                    }
                    if ( i32GPSDatum < 0)
                    { /* Datum not found */
                        if (iDebug & DEP_GPSREC)
                        {
                            fprintf(stderr,"vMsgLoop: Datum Not Found\n");
                        }
		        fclose(fdPLTfile);
		        fdPLTfile = NULL;
                    }
		    else
		    {
		        fgets(stLine, 256, fdPLTfile); /* Altitude is in feet */
		        fgets(stLine, 256, fdPLTfile); /* Reserved 3 */
		        fgets(stLine, 256, fdPLTfile); /* Track data */
		        fgets(stLine, 256, fdPLTfile); /* number of lines */
		    }
		}
	        free(pMsg);
		pMsg = NULL;