recgps.c

open source for car navigation in linux

#ident "@(#) recGPS.c         v1.0.0"
/*
 * Copyright (C) 2002 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.
 ******************************************************************************/


/*Module GPS Reception */

/*****************************************************************************
  This module receives information from GPS, in Garmin or NMEA format,
  changes datum and units, and send information to requesting modules

  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 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 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 */


/* From Demonstration utility for libjeeps
   @author: Copyright (C) Alan Bleasby ([e-mail]) */
/*----------------------------------------------------------------------------*/
/*  FUNCTION :   Init_Jeeps (char *sPort, int p*iTO, int *piProtocol,         */
/*                 key_t *pktQueue, key_t *pktSem)                            */
/*                                                                            */
/*          .-   Initializes the Jeeps library                                */
/*                                                                            */
/* INPUT PARAMETERS        sPort       Port in which the GPS is connected     */
/*                                                                            */
/*                         iTO         Timeout in seconds between info        */
/*                                                                            */
/* OUTPUT PARAMETERS       none                                               */
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static void Init_Jeeps(char *sPort, int *piTO, int *piProtocol, 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. */
    *piProtocol = PROT_GARMIN; /* default GPS comms protocol */
    *pktQueue = GPS_REC_Q; /* default GPS reception module queue key */
    *pktSem = GPS_REC_SEM; /* defalult GPS reception module semaphore key */
    (void) strcpy (sPort, "/dev/ttyS0"); /* default port ttyS0 */

    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,"Init_Jeeps: 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,"Init_Jeeps: line = %s\n",line);
		    }
		    if(*line=='#' || *line=='\n') continue;
		    line[strlen(line)-1]='\0';

		    if(!strncmp(line,"PORT",4))
		    {
			sscanf(&line[4],"%s",sPort);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"Init_Jeeps: PORT = \"%s\"\n",sPort);
		    	}
		    }
		    if(!strncmp(line,"TIMEOUT",7))
		    {
			sscanf(&line[7],"%d",piTO);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"Init_Jeeps: TO = %d\n",*piTO);
		    	}
		    }
		    if(!strncmp(line,"COMMS",5))
		    {
			sscanf(&line[5],"%s",sComms);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"Init_Jeeps: COMMS = \"%s\"\n",sComms);
		    	}
			if(!strncmp(sComms,"GARMIN",6))
			     *piProtocol = PROT_GARMIN;
			else if(!strncmp(sComms,"NMEA",4))
			     *piProtocol = PROT_NMEA;
		     }
		    if(!strncmp(line,"REC_QUEUE",9))
		    {
			sscanf(&line[9],"%d",pktQueue);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"Init_Jeeps: Queue = %d\n",*pktQueue);
		    	}
		    }
		    if(!strncmp(line,"REC_SEM",7))
		    {
			sscanf(&line[7],"%d",pktSem);
			if (iDebug & DEP_GPSREC)
		    	{
			    fprintf(stderr,"Init_Jeeps: 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;

    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,