recgps.c

open source for car navigation in linux

				   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;

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


/*----------------------------------------------------------------------------*/
/*  FUNCTION :   vPrepareGarminData (RGPS_dataMsg *pDataPos,                  */
/*                                   GPS_PPvt_Data pPVT);                     */
/*                                                                            */
/*          .-   Moves data from Garmin PTV strcture to message data structure*/
/*               doinga 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                       */
/*                         pPVT        Pointer to PVT structure from jeeps    */
/*                                     library                                */
/*                         iFirst      = 1, if first call for the same point  */
/*                                      other value, otherwhise               */
/*                                                                            */
/* OUTPUT PARAMETERS       Type of Fix (i.e. 3 = 3D), return <2, no valid data*/
/* 			                                                      */
/*----------------------------------------------------------------------------*/
static int iPrepareGarminData (RGPS_dataMsg *pDataPos,GPS_PPvt_Data pPVT, int iFirst)
{
    double dLat;
    double dLon;

    double dAux;
    int32 i32Aux;
    int32 i32Aux2;

    static double dPrevLat = PREV_DATA_INVALID; /* latitude & longitude of previos point */
    static double dPrevLon;
    static double dDistCalc;

    /* set position in requested datum */
    switch (pDataPos->iDatum)
    {
        case DT_WGS84:
	    dLat = pPVT->lat;
	    dLon = pPVT->lon;
	    pDataPos->dAltitude = pPVT->alt + pPVT->msl_hght;
	    break;
        case DT_EUR50:
	case DT_EUR79:
	case DT_PNIEVES:
            GPS_Math_WGS84_To_Known_Datum_M( pPVT->lat,
	                                         pPVT->lon,
						 (double) pPVT->msl_hght,
						 &dLat,
						 &dLon,
						 &dAux,
						 pDataPos->iDatum);
	    pDataPos->dAltitude = pPVT->alt + pPVT->msl_hght;
	    break;
	default:
	    break;
    }

    /* calculate distance from previous point */
    if(pPVT->fix > 1)
    { /* valid data, 2D or 3D */
        if (iFirst == 0)
	{ /* first calculation for the same point */
            if (dPrevLat == PREV_DATA_INVALID)
	    { /* previous data is invalid */
	        pDataPos->dDist = 0.0;
	    }
	    else
	    { /* previous data valid, calculate distance in metres */
	        dDistCalc = dDist2Points (pPVT->lat, pPVT->lon, dPrevLat, dPrevLon, pDataPos->iDatum);
		pDataPos->dDist = dDistCalc;
	    }
	    dPrevLat = pPVT->lat;
	    dPrevLon = pPVT->lon;
	}
	else
	{ /* no first calculation for same point */
	    pDataPos->dDist = dDistCalc;
	}
    }
    else
    { /* invalid data */
        pDataPos->dDist = 0.0;
	dPrevLat = PREV_DATA_INVALID;
    }

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareGarminData: iDatum:%d\n",pDataPos->iDatum);
	fprintf(stderr,"                    OrgLat: %lf, OrgLon:%lf\n",pPVT->lat, pPVT->lon);
	fprintf(stderr,"                    ConvLat: %lf, ConvLon:%lf\n",dLat, dLon);
	fprintf(stderr,"                    OrgAlt: %f, DestAlt:%lf\n",pPVT->alt, dAux);
	fprintf(stderr,"                    Dist: %lf\n",pDataPos->dDist);
    }

    /* set position in requestet format */
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareGarminData: 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_WGS84_To_UTM_EN_Datum(pPVT->lat,
	                                 pPVT->lon,
	                                 &(pDataPos->RGPSpos.stcUTM.dEast),
	                                 &(pDataPos->RGPSpos.stcUTM.dNorth),
					 &(pDataPos->RGPSpos.stcUTM.lZone),
					 &(pDataPos->RGPSpos.stcUTM.cZC),
					 pDataPos->iDatum) != 0)
            {
	        vLC_syslog(LCLOG_WAR_WGS2UTMERR, __FUNCTION__);
	    }
	    break;
        default:
	    break;
    }

    /* change units as requested */
    dAux = sqrt(pow(pPVT->east,2) + pow(pPVT->east,2) + pow(pPVT->up,2)); /* module of speed */
    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;
    }

    /* calculate bearing */
    pDataPos->dBearing = dBearing (pPVT->east, pPVT->north);

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareGarminData: Units:%d, Aux: %f, Speed:%f\n",pDataPos->btUnits,
	                                                                   dAux,
									   pDataPos->fSpeed);
    }
    
    /* calculate time */
    /* 86400 is the number of secconds in a day */
    pDataPos->tTime = GPS_Math_Gtime_To_Utime(pPVT->tow + pPVT->wn_days * 86400.0 - pPVT->leap_scnds -3600.0);

    return (pPVT->fix);

}


/*----------------------------------------------------------------------------*/
/*  FUNCTION :   iPrepareNMEAData (RGPS_dataMsg *pDataPos, GPS_PGga pGga,     */
/*                                 GPS_PRmc pRmc, GPS_PRmm pRmm)              */
/*                                                                            */
/*          .-   Moves data from NMEA Gga, Rmc and Rmm strctures 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                       */
/*                         pGga:       Pointer to GPS_PGga Jeeps NMEA struct  */
/*                         pRmc:       Pointer to GPS_PRmc Jeeps NMEA struct  */
/*                         pRmm:       Pointer to GPS_PRmm Jeeps NMEA Datum   */
/*                                     structure                              */
/*                         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 iPrepareNMEAData (RGPS_dataMsg *pDataPos, GPS_PGga pGga, GPS_PRmc pRmc, GPS_PRmm pRmm, int iFirst)
{
    double dLat;
    double dLon;

    double dAux;
    int32 i32Aux;
    int32 i32Aux2;

    static double dPrevLat = PREV_DATA_INVALID; /* latitude & longitude of previous point */
    static double dPrevLon;
    static double dDistCalc;

    int32 i32GPSDatum; /* Datum of GPS */
    
    /* Get Datum of GPS */
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareNMEAData: NMEADatum:%s\n",pRmm->datum);
    }
    i32GPSDatum = GPS_Math_Get_Datum_Number(pRmm->datum);
    if ( i32GPSDatum < 0)
    {
        if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"iPrepareNMEAData: Datum Not Found\n");
        }
	return(0);

    }

    if ((int)i32GPSDatum == pDataPos->iDatum)
    {
        dLat = pGga->lat;
	dLon = pGga->lon;
	pDataPos->dAltitude = pGga->alt;
        
	if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"iPrepareNMEAData: Datum are same->no conversion done\n");
        }
    }
    else
    {
        GPS_Math_Known_Datum_To_Known_Datum_M( pGga->lat,
	                                       pGga->lon,
				               (double) pGga->alt,
				               &dLat,
				               &dLon,
				               &dAux,
				               i32GPSDatum,
				               (int32)pDataPos->iDatum);
	pDataPos->dAltitude = pGga->alt;
        
	if (iDebug & DEP_GPSREC)
        {
            fprintf(stderr,"iPrepareNMEAData: GPS datum: %ld, req Datum: %d\n",i32GPSDatum,pDataPos->iDatum);
        }
    }

    /* calculate distance from previous point */
    if(pGga->qual > 0)
    { /* valid data, GPS or DGPS */
        if (iFirst == 0)
	{ /* first calculation for the same point */
            if (dPrevLat == PREV_DATA_INVALID)
	    { /* previous data is invalid */
	        pDataPos->dDist = 0.0;
	    }
	    else
	    { /* previous data valid, calculate distance in metres */
	        dDistCalc = dDist2Points (pGga->lat, pGga->lon, dPrevLat, dPrevLon, pDataPos->iDatum);
		pDataPos->dDist = dDistCalc;
	    }
	    dPrevLat = pGga->lat;
	    dPrevLon = pGga->lon;
	}
	else
	{ /* no first calculation for same point */
	    pDataPos->dDist = dDistCalc;
	}
    }
    else
    { /* invalid data */
        pDataPos->dDist = 0.0;
	dPrevLat = PREV_DATA_INVALID;
    }

    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareNMEAData:   iDatum:%d\n",pDataPos->iDatum);
	fprintf(stderr,"                    OrgLat: %lf, OrgLon:%lf\n",pGga->lat, pGga->lon);
	fprintf(stderr,"                    ConvLat: %lf, ConvLon:%lf\n",dLat, dLon);
	fprintf(stderr,"                    OrgAlt: %f, DestAlt:%lf\n",pGga->alt, pDataPos->dAltitude);
	fprintf(stderr,"                    Dist: %lf\n",pDataPos->dDist);
    }

    /* set position in requestet format */
    if (iDebug & DEP_GPSREC)
    {
        fprintf(stderr,"iPrepareNMEAData: 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(pGga->lat,
	                                 pGga->lon,
	                                 &(pDataPos->RGPSpos.stcUTM.dEast),
	                                 &(pDataPos->RGPSpos.stcUTM.dNorth),
					 &(pDataPos->RGPSpos.stcUTM.lZone),
					 &(pDataPos->RGPSpos.stcUTM.cZC),