readozitrack.c

open source for car navigation in linux

	        break;

	    default:
	        free(pMsg);
		pMsg = NULL;
		break;
	}
    }
}


/*----------------------------------------------------------------------------*/
/*  FUNCTION :   iPrepareLineData (RGPS_dataMsg *pDataPos,                    */
/*                                 char *stLine, int iFirst)                  */
/*                                                                            */
/*          .-   Moves data read PLT line to message data structure doing all */
/*               needed conversions (datum, pos format, units, ..)            */
/*                                                                            */
/* INPUT PARAMETERS        pDataPos:   Pointer to RGPS_dataMsg message struct */
/*                                     initialized with datum, pos_format and */
/*                                     units values.                          */
/*                                     The function fills the remaining data  */
/*                                     in the structure                       */
/*                         stLine      string containing PLT readed line      */
/*                         iFirst      = 1, if first call for the same point  */
/*                                      other value, otherwhise               */
/*                                                                            */
/* OUTPUT PARAMETERS       Type of Fix (i.e.1 = GPS), return <1, no valid data*/
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static int iPrepareLineData (RGPS_dataMsg *pDataPos, char *stLine, int iFirst)
{
    double dLat;
    double dLon;

    double dLineLat, dLineLong, dLineAlt, dLineTime = 0;
    int iLineFirst;

    double dAux;
    char *stAux;
    int32 i32Aux;
    int32 i32Aux2;

    static double dPrevLat = PREV_DATA_INVALID; /* latitude & longitude of previous point */
    static double dPrevLon;
    static double dDistCalc;
    static time_t tPrevTime = 0; /* time of previous point */
    static double dPrevBearing = 0;
    long lDeltaT; /* time difference between previos point and current */

    /* extract latitude, longitude and time */
    dLineLat = strtod(stLine, &stAux);
    dLineLong = strtod(stAux+1, &stAux);
    iLineFirst = strtoul(stAux+1, &stAux, 10);
    dLineAlt = strtod(stAux+1, &stAux);
    dLineTime = strtod(stAux+1, &stAux);
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareLineData: LineLat:%lf, LineLong:%lf, LineFirst:%d, LineAlt:%lf, LineTime:%lf\n",
                       dLineLat, dLineLong, iLineFirst, dLineAlt, dLineTime);
    }

    if ((int)i32GPSDatum == pDataPos->iDatum)
    {
        dLat = dLineLat;
	dLon = dLineLong;
	pDataPos->dAltitude = FEET2M(dLineAlt);

	if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"iPrepareLineData: Datum are same->no conversion done\n");
        }
    }
    else
    {
        GPS_Math_Known_Datum_To_Known_Datum_M( dLineLat,
	                                       dLineLong,
				               (double) FEET2M(dLineAlt),
				               &dLat,
				               &dLon,
				               &dAux,
				               i32GPSDatum,
				               (int32)pDataPos->iDatum);
	pDataPos->dAltitude = FEET2M(dLineAlt);

	if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"iPrepareLineData: GPS datum: %ld, req Datum: %d\n",i32GPSDatum,pDataPos->iDatum);
        }
    }

    /* calculate distance from previous point and bearing */
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareLineData: iLineFirst=%d, iFirst=%d\n", iLineFirst, iFirst);
    }
    if(iLineFirst == 0)
    { /* no first point in track */
        if (iFirst == 0)
	{ /* first calculation for the same point */
	    dDistCalc = dDist2Points (dLineLat, dLineLong, dPrevLat, dPrevLon, pDataPos->iDatum);
            if (iDebug & DEP_GPSREC)
            {
                fprintf(stderr,"iPrepareLineData: dDistCalc=%lf\n", dDistCalc);
            }
	    if (dDistCalc != 0)
	    { /* change of position, calculate bearing */
	        pDataPos->dBearing = dBearing ((float) (dLineLong-dPrevLon), (float) (dLineLat-dPrevLat));
	        dPrevBearing = pDataPos->dBearing;
	    }
	    pDataPos->dDist = dDistCalc;
	    dPrevLat = dLineLat;
	    dPrevLon = dLineLong;
	}
	else
	{ /* no first calculation for same point */
	    pDataPos->dDist = dDistCalc;
	    pDataPos->dBearing = dPrevBearing;
	}
    }
    else
    { /* first point in track */
        pDataPos->dDist = 0.0;
	dPrevLat = dLineLat;
	dPrevLon = dLineLong;
	pDataPos->dBearing = dPrevBearing;
    }

    /* Time from previous point */
    if(iLineFirst == 0)
    { /* no first point in track */
        if (tPrevTime == 0)
	{ /* no previos time */
	    lDeltaT = -1;
	}
	else
	{ /* normal point */
	    lDeltaT = tWindatetime2Unixtime(dLineTime) - tPrevTime;
	}
    }
    else
    { /* first line in segment/track */
        lDeltaT = -1;
    }
    tPrevTime = tWindatetime2Unixtime(dLineTime);
    
    /* Get position time */
    pDataPos->tTime = tWindatetime2Unixtime(dLineTime);

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareLineData: DeltaT=%ld, Time= %ld\n", lDeltaT, pDataPos->tTime);
    }

    /* Speed calculation */
    if (lDeltaT < 0)
    { /* no valid time increment */
        pDataPos->fSpeed = -1.0;
    }
    else
    {
        pDataPos->fSpeed = pDataPos->dDist / lDeltaT;
    }

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareLineData:   iDatum:%d\n",pDataPos->iDatum);
	fprintf(stderr,"                    OrgLat: %lf, OrgLon:%lf\n",dLineLat, dLineLong);
	fprintf(stderr,"                    ConvLat: %lf, ConvLon:%lf\n",dLat, dLon);
	fprintf(stderr,"                    OrgAlt: %f, DestAlt:%lf\n",FEET2M(dLineAlt), pDataPos->dAltitude);
	fprintf(stderr,"                    Dist: %lf\n",pDataPos->dDist);
	fprintf(stderr,"                    Speed: %f Km/h\n",MS2KPH(pDataPos->fSpeed));
	fprintf(stderr,"                    Bearing: %lf\n",pDataPos->dBearing);
    }

    /* set position in requestet format */
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareLineData: PosFormat:%d\n",pDataPos->btPosFormat);
    }
    switch (pDataPos->btPosFormat)
    {
        case PF_HD:
            pDataPos->RGPSpos.stcD.dNorth = dLat;
            pDataPos->RGPSpos.stcD.dEast = dLon;
            break;
        case PF_HD_M:
	    GPS_Math_Deg_To_DegMin(dLat, &i32Aux, &dAux);
	    pDataPos->RGPSpos.stcDM.iDNorth = i32Aux;
	    pDataPos->RGPSpos.stcDM.dMNorth = dAux;
	    GPS_Math_Deg_To_DegMin(dLon, &i32Aux, &dAux);
	    pDataPos->RGPSpos.stcDM.dMEast = i32Aux;
	    pDataPos->RGPSpos.stcDM.iDEast = dAux;
	    break;
        case PF_HD_M_S:
	    GPS_Math_Deg_To_DegMinSec(dLat,
	                              &i32Aux,
	                              &i32Aux2,
				      &dAux);
	    pDataPos->RGPSpos.stcDMS.iDNorth = i32Aux;
	    pDataPos->RGPSpos.stcDMS.iMNorth = i32Aux2;
	    pDataPos->RGPSpos.stcDMS.fSNorth = (float) dAux;
	    GPS_Math_Deg_To_DegMinSec(dLon,
	                              &i32Aux,
				      &i32Aux2,
				      &dAux);
	    pDataPos->RGPSpos.stcDMS.iDEast = i32Aux;
	    pDataPos->RGPSpos.stcDMS.iMEast = i32Aux2;
	    pDataPos->RGPSpos.stcDMS.fSEast = (float) dAux;
	    break;
        case PF_UTM:
	    if (GPS_Math_Know_Datum_To_UTM_EN_Datum(dLat,
	                                 dLon,
	                                 &(pDataPos->RGPSpos.stcUTM.dEast),
	                                 &(pDataPos->RGPSpos.stcUTM.dNorth),
					 &(pDataPos->RGPSpos.stcUTM.lZone),
					 &(pDataPos->RGPSpos.stcUTM.cZC),
					 i32GPSDatum,
					 pDataPos->iDatum) != 0)
            {
	        vLC_syslog(LCLOG_WAR_WGS2UTMERR, __FUNCTION__);
	    }
	    break;
        default:
	    break;
    }

    /* change units as requested */
    dAux = pDataPos->fSpeed;; /* 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 = MS2KNOT(dAux);
	    pDataPos->dDist = M2NMILE(pDataPos->dDist);
	    break;
	default:
	    break;
    }

    return (1);

}
/*----------------------------------------------------------------------------*/
/*  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: readOzitrack                                            */
/*----------------------------------------------------------------------------*/
int main(int argc, char **argv)
{
    int iTO;
    key_t ktQueue; /* MSG queue key */
    key_t ktSem; /* Semaphore for thread syncronisation */
    
    char stLine[256]; /* for store lines read from PLT file */

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

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

    int i, iRes;
    
    
    // *************** 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 ******************
    vGetInitData(&iTO, &ktQueue, &ktSem);

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"readOziTrack: iTO=%d, ktQueue=%d, ktSem=%d\n",iTO,ktQueue, ktSem);
    }

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

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


    //*************** Loop and read data **********************************************
    for(;;)
    { /* loop */

	sleep(iTO);

	/* read data from file */
	if (fdPLTfile != NULL)
	{ /* file open */
            if (fgets(stLine, 256, fdPLTfile) == NULL)
	    { /* end of file */
	        fclose(fdPLTfile);
		fdPLTfile = NULL;
            }
	    else
	    { /* line read, process it */
		stLine[strlen(stLine)-2] = '\0'; /* eliminates <CR> <LF> characters */
                if (iDebug & DEP_GPSREC)
                {
                    fprintf(stderr,"readOzitrack/main, line read=[%s]\n", stLine);
	        }
	        /* start of exclusive area */
	        if (iLockSem(iSemId) != 0)
	        {
	            vLC_syslog(LCLOG_ERR_SEMLK, "readOziTrack/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,"readOziTrack");
                    }
                    else
                    { /* Init DataPos structure */
		        if (iDebug & DEP_GPSREC)
		        {
		            fprintf(stderr,"readOziTrack/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 */
                    }

	            iRes = iPrepareLineData (pDataPos, stLine, i);
		    if (iDebug & DEP_GPSREC)
		    {
		        vPrint_Debug_DataPos(pDataPos, "readOziTrack");
		    }
		    if (iRes > 0 )
		    {/* Fix > 0 -> valid data */
		        iRes = iSend2Queue(pConList[i].iQid, pDataPos, sizeof(RGPS_dataMsg));
		        if (iDebug & DEP_GPSREC)
		        {
		            fprintf(stderr,"readOziTrack: Queue: %d, (%d)\n",pConList[i].iQid, pConList[i].ktQueue);
		        }
		        if (iRes < 0)
		        { /* Error, remove this connection */
			    if (iDebug & DEP_GPSREC)
			    {
				fprintf(stderr,"readOziTrack: 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,"readOziTrack/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, "readOziTrack/main", iSemId);
	            exit(-1);
	        }
	    }
	}

    } /* end loop */


    return 0;

}