recgps.c

open source for car navigation in linux

					 i32GPSDatum,
					 pDataPos->iDatum) != 0)
            {
	        vLC_syslog(LCLOG_WAR_WGS2UTMERR, __FUNCTION__);
	    }
	    break;
        default:
	    break;
    }

    /* change units as requested */
    dAux = KNOT2MS(pRmc->speed); /* speed in m/s */
    switch (pDataPos->btUnits)
    {
        case UD_METRIC:
	    pDataPos->fSpeed = MS2KPH(dAux);
	    break;
	case UD_ESTATUATE:
	    pDataPos->fSpeed = MS2MPH(dAux);
	    pDataPos->dAltitude = M2FEET(pDataPos->dAltitude);
	    pDataPos->dDist = M2FEET(pDataPos->dDist);
	    break;
	case UD_NAUTIC:
	    pDataPos->fSpeed = pRmc->speed;
	    pDataPos->dDist = M2NMILE(pDataPos->dDist);
	    break;
	default:
	    break;
    }

    /* calculate bearing */
    pDataPos->dBearing = pRmc->cmg;

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareNMEAData: Units:%d, Aux: %f, Speed:%f\n",pDataPos->btUnits,
	                                                                   dAux,
									   pDataPos->fSpeed);
    }
    
    /* Get position time */
    pDataPos->tTime = pGga->time;

    return (pGga->qual);

}
/*----------------------------------------------------------------------------*/
/*  FUNCTION :   vExitHandler(int iSigId)                                     */
/*                                                                            */
/*          .-   Handles the SIGTERM SIGNAL.                                  */
/*               deletes rec_GPS input queue                                  */
/*                                                                            */
/* INPUT PARAMETERS        iSigId:     Signal identifier (SIGTERM)            */
/*                                                                            */
/* OUTPUT PARAMETERS       none                                               */
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static void vExitHandler(int iSigId)
{

    // cancel thread MsgLoop
    pthread_cancel(thtLoopThread);

    // delete recGPS input Queue
    iDelQmsg(iQid);
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"vExitHandler: signal=%d\n",iSigId);
    }
    exit(0);
}

/*----------------------------------------------------------------------------*/
/*             main program                                                   */
/*             Usage: recGPS                                                  */
/*----------------------------------------------------------------------------*/
int main(int argc, char **argv)
{
    char sPort[MAXPATHLEN];
    int iTO;
    int iProtocol;
    key_t ktQueue; /* MSG queue key */
    key_t ktSem; /* Semaphore for thread syncronisation */

    int32 iRes;
    int32 fd;
    int i;

    int iThArgNotUsed = 0; /*argument passed to thread start routine, not used */

    RGPS_dataMsg *pDataPos = NULL; /*pointer to data structure */

    GPS_PPvt_Data pPVT = NULL; /* pointer to PTV data */

    GPS_PGga pGga = NULL; /* pointer to NMEA Fix data */
    GPS_PRmc pRmc = NULL; /* pointer to NMEA GPS data */
    GPS_PRmm pRmm = NULL; /* pointer to NMEA datum */


    // *************** Check for Debug environment variable ****************
    iDebug = ( getenv("GPSNAV_DEBUG") == (char *)0 ? 0 : atoi(getenv("GPSNAV_DEBUG")));

    // register SIGTERM handler
    signal(SIGTERM,vExitHandler);

    // *************** Get parameters & init dtata & structures ******************
    Init_Jeeps(sPort, &iTO, &iProtocol, &ktQueue, &ktSem);

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"recGPS: sPort=%s, iTO=%d, iProtocol=%d, ktQueue=%d, ktSem=%d\n",sPort,iTO,iProtocol,ktQueue, ktSem);
    }
    vLC_syslog(LCLOG_WAR_GPSRECSTART, PACKAGE, VERSION, sPort, iTO, (iProtocol == PROT_GARMIN ? "GARMIN" : "NMEA") );

    
    //********* Initialize IPC and create thread to wait for messages ******************
    iQid = iCreateQmsg(ktQueue);
    if (iQid < 0)
    {
        vLC_syslog(LCLOG_ERR_MSGQCREATE, "recGPS/main", ktQueue);
	exit(-1);
    }

    iSemId = iCreateSem(ktSem);
    if (iSemId < 0)
    {
      vLC_syslog(LCLOG_ERR_SEMGET, "recGPS/main", ktSem);
	exit(-1);
    }

    if (pthread_create( &thtLoopThread, NULL, (void *)vMsgLoop, (void *)&iThArgNotUsed) < 0)
    { /* thread create error */
        vLC_syslog(LCLOG_ERR_THREADCREATE, "recGPS/main", errno);
	exit(-1);
    }



    //*********************  Init GPS comms ******************************
    if ( iProtocol == PROT_GARMIN)
    { /* Garmin protocol */
//        if(GPS_Init(sPort)<0)
        while(GPS_Init(sPort)<0)
        {
//            vLC_syslog(LCLOG_ERR_NOGPSINIT, sPort);
            if (iDebug & DEP_GPSREC)
            {
                fprintf(stderr,"recGPS: GPS unavailable, wait...\n");
            }
//	    exit(-1);
            sleep(iTO);
        }
        // PVT constructor
        if(!(pPVT = GPS_Pvt_New()))
        { /* Error creating PTV object */
            vLC_syslog(LCLOG_ERR_PTVCONS, "recGPS");
	    exit(-1);
        }

        // PVT on
        if((iRes=GPS_Command_Pvt_On(sPort,&fd)) <= 0)
        { /* PVT on error */
            vLC_syslog(LCLOG_ERR_PTVON, "recGPS", iRes);
            exit(-1);
        }

    }
    else if (iProtocol == PROT_NMEA)
    { /* NMEA protocol */
        GPS_NMEA_Init(sPort);

	pGga = GPS_Gga_New(); /* create Gga object */
	pRmc = GPS_Rmc_New(); /* create Rmc object */
	pRmm = GPS_Rmm_New(); /* create Rmm object */

//	GPS_Enable_Diagnose();
    }


    //*************** Loop and read GPS data **********************************************
    for(;;)
    { /* loop */
        if (iProtocol != PROT_GARMIN)
	{
	    sleep(iTO);
	}

	if ( iProtocol == PROT_GARMIN)
        { /* Garmin protocol */
            GPS_Command_Pvt_Get(&fd,&pPVT);
            if (iDebug & DEP_GPSREC)
            {
                GPS_Fmt_Print_Pvt(pPVT,stdout);
            }
	    if (pPVT->fix == 0)
	    { /* GPS unavailable, restart PVT */
	        if (iDebug & DEP_GPSREC)
	        {
	            fprintf(stderr,"recGPS: GPS unavailable, restart PVT\n");
	        }

		if(GPS_Command_Pvt_On(sPort,&fd) <= 0)
		{ /* PVT failure, GPS maybe still off */
		    if (iDebug & DEP_GPSREC)
		    {
		        fprintf(stderr,"recGPS: PVT on, failure\n");
		    }
		}
	    }
        }
        else if ( iProtocol == PROT_NMEA)
        { /* NMEA protocol */
	    GPS_NMEA_Get_Gga (&pGga);
            if (iDebug & DEP_GPSREC)
            {
                vPrint_Debug_NMEA_Gga(pGga);
            }
	    GPS_NMEA_Get_Rmc (&pRmc);
            if (iDebug & DEP_GPSREC)
            {
                vPrint_Debug_NMEA_Rmc(pRmc);
            }
	    GPS_NMEA_Get_Rmm (&pRmm);
            if (iDebug & DEP_GPSREC)
            {
                vPrint_Debug_NMEA_Rmm(pRmm);
            }
	}

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

	for (i=0; i < iConNo; i++)
	{ /* loops for each active connection */

            // Create and init position structure

            if((pDataPos = malloc(sizeof(RGPS_dataMsg))) == NULL)
            { /* malloc error */
                vLC_syslog(LCLOG_ERR_MALLOC,"recGPS");
            }
            else
            { /* Init DataPos structure */
		if (iDebug & DEP_GPSREC)
		{
		    fprintf(stderr,"recGPS/main: i=%d, pDataPos=%p\n",i,pDataPos);
		}
		pDataPos->mtype = MSG_DATA;
		pDataPos->iRGPSid = GPSSEND_POS;
		pDataPos->iDatum = pConList[i].iDatum;
		pDataPos->btUnits = pConList[i].btUnits;
		pDataPos->btPosFormat = pConList[i].btPosFormat;
		pDataPos->shNumSat = -1; /* -1 means don't know no. satellites */
		pDataPos->dDist = -1; /* N/A */
		pDataPos->fSpeed = -1; /* N/A */
            }

	    if(iProtocol == PROT_GARMIN)
	    {
	        iRes = iPrepareGarminData (pDataPos, pPVT, i);
		if (iDebug & DEP_GPSREC)
		{
		    vPrint_Debug_DataPos(pDataPos, "recGPS");
		}
		if (iRes > 1 )
		{/* Fix > 1 -> valid data */
		    iRes = iSend2Queue(pConList[i].iQid, pDataPos, sizeof(RGPS_dataMsg));
		    if (iDebug & DEP_GPSREC)
		    {
		        fprintf(stderr,"recGPS: Queue: %d, (%d)\n",pConList[i].iQid, pConList[i].ktQueue);
		    }
		    if (iRes < 0)
		    { /* Error, remove this connection */
			if (iDebug & DEP_GPSREC)
			{
				fprintf(stderr,"recGPS: Remove connection: Queue=%d\n",pConList[i].ktQueue);
			}
		        vRemoveConn(pConList[i].ktQueue);
			i--; /* set active connection same as current */
		    }
		    else
		    { /* data sent, memory free, set pointer to NULL */
		        pDataPos = NULL;
		    }
		}
		else
		{
		    if (iDebug & DEP_GPSREC)
		    {
		        fprintf(stderr,"recGPS/main: No data sent to connection %d, pDataPos=%p\n",i, pDataPos);
		    }
		    free(pDataPos);
		    pDataPos = NULL;

		}
	    }

	    if(iProtocol == PROT_NMEA)
	    {
	        iRes = iPrepareNMEAData (pDataPos, pGga, pRmc, pRmm, i);
		if (iDebug & DEP_GPSREC)
		{
		    vPrint_Debug_DataPos(pDataPos, "recGPS");
		}
		if (iRes > 0 )
		{/* Fix > 0 -> valid data */
		    iRes = iSend2Queue(pConList[i].iQid, pDataPos, sizeof(RGPS_dataMsg));
		    if (iDebug & DEP_GPSREC)
		    {
		        fprintf(stderr,"recGPS: Queue: %d, (%d)\n",pConList[i].iQid, pConList[i].ktQueue);
		    }
		    if (iRes < 0)
		    { /* Error, remove this connection */
			if (iDebug & DEP_GPSREC)
			{
				fprintf(stderr,"recGPS: Remove connection: Queue=%d\n",pConList[i].ktQueue);
			}
		        vRemoveConn(pConList[i].ktQueue);
			i--; /* set active connection same as current */
		    }
		    else
		    { /* data sent, memory free, set pointer to NULL */
		        pDataPos = NULL;
		    }
		}
		else
		{
		    if (iDebug & DEP_GPSREC)
		    {
		        fprintf(stderr,"recGPS/main: No data sent to connection %d, pDataPos=%p\n",i, pDataPos);
		    }
		    free(pDataPos);
		    pDataPos = NULL;
		}
	    }


	    if(pDataPos != NULL) free(pDataPos);

	} /* end active connections loop */

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


    } /* end loop */


    //Close GPS comms & switch off GPS
    if ( iProtocol == PROT_GARMIN)
    { /* Garmin protocol */
        if((iRes=GPS_Command_Off(sPort))<0)
        {
            vLC_syslog(LCLOG_ERR_NOGPSOFF, sPort, iRes);
        }
        //PTV off
        GPS_Command_Pvt_Off(sPort,&fd);
        GPS_Pvt_Del(&pPVT); /*destroy PTV data structure */

    }
    else if ( iProtocol == PROT_NMEA)
    { /* NMEA protocol */
        GPS_NMEA_Exit();

	GPS_Gga_Del(&pGga); /* delete Gga object */
	GPS_Rmc_Del(&pRmc); /* delete Rmc object */
	GPS_Rmm_Del(&pRmm); /* delete Rmm object */
    }

    return 0;


}