gps.c

Open DMT Client C Source code

            //      6    A=valid, V=invalid
            // $GPGSA - GPS DOP and Active Satellites
            //   $GPGSA|A|1|||||||||||||50.0|50.0|50.0*05|
            //   $GPGSA|A|3|16|20|13|23|||||||||4.3|2.3|3.7*36|
            //   $GPGSA,1,2, 3, 4, 5, 6,  ...    15, 16, 17
            //      1    A =Auto2D/3D, M=Forced2D/3D
            //      2    1=NoFix, 2=2D, 3=3D
            //      3    Satellites used #1
            //    ...
            //     14    Satellites used #12
            //     15    PDOP
            //     16    HDOP
            //     17    VDOP
            // $GPGSV - GPS Satellites in View
            //   $GPGSV|3|1|9|16|35|51|32|4|9|269|0|20|32|177|33|13|62|329|37*4E|
            //   $GPGSV|3|2|9|3|14|113|0|24|5|230|0|8|12|251|0|23|71|71|39*70|
            //   $GPGSV|3|3|9|131|0|0|0*43|
            // $GPRMC - Recommended Minimum Specific GPS/TRANSIT Data
            //   $GPRMC|015402.240|V|36000.0000|N|72000.0000|E|0.000000||200505||*3C|
            //   $GPRMC,025423.494,A,3709.0642,N,11907.8315,W,0.094824,108.52,200505,,*12
            //   $GPRMC,      1   ,2,    3    ,4,     5    ,6, 7      ,   8  ,  9   ,A,B*M
            //      1   UTC time of position HHMMSS
            //      2   validity of the fix ("A" = valid, "V" = invalid)
            //      3   current latitude in ddmm.mm format
            //      4   latitude hemisphere ("N" = northern, "S" = southern)
            //      5   current longitude in dddmm.mm format
            //      6   longitude hemisphere ("E" = eastern, "W" = western)
            //      7   speed in knots
            //      8   true course in degrees
            //      9   date in DDMMYY format
            //      A   magnetic variation in degrees
            //      B   direction of magnetic variation ("E" = east, "W" = west)
            //      M   checksum
            // $GPVTG - Track Made Good and Ground Speed
            //   $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*41
            //             1  2    3  4    5  6    7  8
            //      1   True course made good over ground, degrees
            //      2   Magnetic course made good over ground, degrees
            //      3   Ground speed, N=Knots
            //      4   Ground speed, K=Kilometers per hour

            utBool validFix_GPRMC = utFalse;
            utBool validFix_GPGGA = utFalse;
            char fldData[256], *fld[22];
            memset(fld, 0, sizeof(fld));
            strncpy(fldData, data, sizeof(fldData) - 1); // local copy
            fldData[sizeof(fldData) - 1] = 0; // safety net terminator
            strParseArray(fldData, fld, 20);

            /* checksum (XOR sum of all characters between '$' and '*', exclusive) */
            if (!cksumIsValidCharXOR(&data[1],(int*)0)) {
                logWARNING(LOGSRC,"GPS record failed checksum: %s", fld[0]);
                continue; // skip this record
            }

            /* handle record type */
            // We cannot assume that the GPRMC will arrive before GPGGA, or visa-versa.
            // so we must handle either case.
            if (strEqualsIgnoreCase(fld[0],"$GPRMC")) {
#if defined(SUPPORT_TYPE_GPRMC)
                if (*fld[2] == 'A') {
                    // "A" - valid gps fix
                    
                    /* parse */
                    UInt32 hms       = strParseUInt32(fld[1], 0L);
                    UInt32 dmy       = strParseUInt32(fld[9], 0L);
                    UInt32 fixtime   = _gpsGetUTCSeconds(dmy, hms);
                    double latitude  = getLatitude (fld[3], fld[4]);
                    double longitude = getLongitude(fld[5], fld[6]);
                    double knots     = strParseDouble(fld[7], -1.0);
                    double heading   = strParseDouble(fld[8], -1.0);
                    double speedKPH  = (knots >= 0.0)? (knots * KILOMETERS_PER_KNOT) : -1.0;
                    
                    /* HDOP expired? */
                    if (utcIsTimerExpired(gpsLastHDOPTimer,60L)) {
                        gpsLastPDOP = GPS_UNDEFINED_DOP;
                        gpsLastHDOP = GPS_UNDEFINED_DOP;
                        gpsLastVDOP = GPS_UNDEFINED_DOP;
                        // do not reset timer
                    }
                    
                    /* save */
                    if (fixtime < MIN_CLOCK_TIME) {
                        
                        // The fixtime is invalid
                        char dt[32];
                        utcFormatDateTime(dt, fixtime);
                        logERROR(LOGSRC,"$GPRMC invalid fixtime: [%lu] %s UTC", fixtime, dt);
                        logERROR(LOGSRC,"GPS: %s", data);
                        
                    } else
                    if ((latitude  >=  90.0) || (latitude  <=  -90.0) ||
                        (longitude >= 180.0) || (longitude <= -180.0)   ) {
                            
                        // We have an valid record, but the lat/lon appears to be invalid!
                        logWARNING(LOGSRC,"$GPRMC invalid lat/lon: %.5lf/%.5lf", latitude, longitude);
                        
                    } else {
                        
                        GPS_LOCK {
                            // If the $GPGGA came first, we don't want to indiscriminately clear
                            // out the GPS structure before adding the $GPRMC data.
                            if ((gpsFixUnsafe.fixtype == 0)                  ||     // no GPS fix
                                (gpsFixUnsafe.fixtime < MIN_CLOCK_TIME)      ||     // uninitialized fixtime
                                (gpsFixUnsafe.fixtime > fixtime)             ||     // fixtime in the future
                                ((fixtime - gpsFixUnsafe.fixtime) > GP_EXPIRE) ) {  // expired fixtime
                                gpsClear(&gpsFixUnsafe);
                                gpsFixUnsafe.fixtype = 1; // GPS
                            } else
                            if ((gpsFixUnsafe.point.latitude  != latitude ) ||      // latitude  changed (no GGA?)
                                (gpsFixUnsafe.point.longitude != longitude)   ) {   // longitude changed (no GGA?)
                                gpsClear(&gpsFixUnsafe);
                                gpsFixUnsafe.fixtype = 1; // GPS
                            } else {
                                // I will be adding to the existing data
                            }
                            gpsFixUnsafe.ageTimer        = utcGetTimer();
                            gpsFixUnsafe.fixtime         = fixtime;
                            gpsFixUnsafe.speedKPH        = speedKPH;
                            gpsFixUnsafe.heading         = heading;
                            gpsFixUnsafe.point.latitude  = latitude;
                            gpsFixUnsafe.point.longitude = longitude;
                            gpsFixUnsafe.pdop            = gpsLastPDOP;
                            gpsFixUnsafe.hdop            = gpsLastHDOP;
                            gpsFixUnsafe.vdop            = gpsLastVDOP;
                            gpsFixUnsafe.nmea           |= NMEA0183_GPRMC;
                            if (gpsFixUnsafe.nmea & NMEA0183_GPGGA) {
                                gpsCopy(&gpsFixLast, &gpsFixUnsafe);
                            }
                        } GPS_UNLOCK
                        validFix_GPRMC = utTrue;

                        /* update system clock ($GPRMC records only!) */
                        gpsUpdateSystemClock(fixtime + 2L);

                    }
                            
                    /* count valid record type */
                    SAMPLE_LOCK { // locked to prevent external access
                        gpsSampleCount_A++;
                        if (gpsLastSampleTimer == 0L) {
                            logINFO(LOGSRC,"First $GPRMC record (A) @%lu", fixtime);
                        } 
                        gpsLastSampleTimer = utcGetTimer();
                        if (validFix_GPRMC) { gpsLastValidTimer = utcGetTimer(); }
                    } SAMPLE_UNLOCK

                } else {
                    // "V" - invalid gps fix
                        
                    /* count invalid record type */
                    SAMPLE_LOCK { // locked to prevent external access
                        gpsSampleCount_V++;
                        if (gpsLastSampleTimer == 0L) {
                            logINFO(LOGSRC,"First $GPRMC record (V) @%lu", utcGetTimeSec());
                        } 
                        gpsLastSampleTimer = utcGetTimer();
                    } SAMPLE_UNLOCK

                    // No valid fix aquired

                }
#endif
            } else
            if (strEqualsIgnoreCase(fld[0],"$GPGGA")) {
#if defined(SUPPORT_TYPE_GPGGA)
                if (*fld[6] != '0') {
                    // "1" = GPS
                    // "2" = DGPS

                    /* parse */
                    UInt32 hms       = strParseUInt32(fld[1], 0L);
                    UInt32 dmy       = 0L; // we don't know the day
                    UInt32 fixtime   = _gpsGetUTCSeconds(dmy, hms);
                    double latitude  = getLatitude (fld[2], fld[3]);
                    double longitude = getLongitude(fld[4], fld[5]);
                    gpsLastHDOP      = strParseDouble(fld[8], GPS_UNDEFINED_DOP);
                    int    fixtype   = (int)strParseUInt32(fld[6], 1L); // 1=GPS, 2=DGPS, 3=PPS?, ...
                    double altitude  = strParseDouble(fld[9], 0.0);

                    /* PDOP/VDOP expired? */
                    if (utcIsTimerExpired(gpsLastHDOPTimer,60L)) {
                        gpsLastPDOP = GPS_UNDEFINED_DOP;
                        gpsLastVDOP = GPS_UNDEFINED_DOP;
                    }
                    gpsLastHDOPTimer = utcGetTimer(); // reset timer

                    /* save */
                    if (fixtime < MIN_CLOCK_TIME) {
                        // The fixtime is invalid
                        // (this can occur if the system clock time hasn't yet been updated)
                        char dt[32];
                        utcFormatDateTime(dt, fixtime);
                        logINFO(LOGSRC,"$GPGGA invalid fixtime: [%lu] %s UTC", fixtime, dt);
                        logINFO(LOGSRC,"GPS: %s", data);
                    } else
                    if ((latitude  >=  90.0) || (latitude  <=  -90.0) ||
                        (longitude >= 180.0) || (longitude <= -180.0)   ) {
                        // We have an valid record, but the lat/lon appears to be invalid!
                        logWARNING(LOGSRC,"$GPGGA invalid lat/lon: %.5lf/%.5lf", latitude, longitude);
                    } else {
                        GPS_LOCK {
                            // If the $GPRMC came first, we don't want to indiscriminately clear
                            // out the GPS structure before adding the $GPGGA data.
                            if ((gpsFixUnsafe.fixtype == 0)                  ||      // no GPS fix
                                (gpsFixUnsafe.fixtime < MIN_CLOCK_TIME)      ||      // uninitialized fixtime
                                (gpsFixUnsafe.fixtime > fixtime)             ||      // fixtime in the future
                                ((fixtime - gpsFixUnsafe.fixtime) > GP_EXPIRE) ) {   // expired fixtime
                                gpsClear(&gpsFixUnsafe);
                            } else
                            if ((gpsFixUnsafe.point.latitude  != latitude ) ||      // latitude  changed (no GGA?)
                                (gpsFixUnsafe.point.longitude != longitude)   ) {   // longitude changed (no GGA?)
                                //gpsClear(&gpsFixUnsafe);
                            } else {
                                // I will be adding to the existing data
                            }
                            gpsFixUnsafe.ageTimer        = utcGetTimer();
                            gpsFixUnsafe.fixtime         = fixtime;
                            gpsFixUnsafe.altitude        = altitude;
                            gpsFixUnsafe.point.latitude  = latitude;
                            gpsFixUnsafe.point.longitude = longitude;
                            gpsFixUnsafe.fixtype         = fixtype;
                            gpsFixUnsafe.pdop            = gpsLastPDOP;
                            gpsFixUnsafe.hdop            = gpsLastHDOP;
                            gpsFixUnsafe.vdop            = gpsLastVDOP;
                            gpsFixUnsafe.nmea           |= NMEA0183_GPGGA;
                            if (gpsFixUnsafe.nmea & NMEA0183_GPRMC) {
                                gpsCopy(&gpsFixLast, &gpsFixUnsafe);
                            }
                        } GPS_UNLOCK
                        validFix_GPGGA = utTrue;
                    }
                            
                    /* count valid record type */
                    SAMPLE_LOCK { // locked to prevent external access to 'gpsLastValidTimer'
                        if (validFix_GPGGA) { gpsLastValidTimer = utcGetTimer(); }
                    } SAMPLE_UNLOCK

                }
#endif
            } else
            if (strEqualsIgnoreCase(fld[0],"$GPGSA")) {
#if defined(SUPPORT_TYPE_GPGSA)
                if ((*fld[2] != '2') || (*fld[2] != '3')) {
                    // "2" = 2D
                    // "3" = 3D

                    /* parse */
                    gpsLastPDOP = strParseDouble(fld[15], GPS_UNDEFINED_DOP);
                    gpsLastHDOP = strParseDouble(fld[16], GPS_UNDEFINED_DOP);
                    gpsLastVDOP = strParseDouble(fld[17], GPS_UNDEFINED_DOP);
                    gpsLastHDOPTimer = utcGetTimer();
                    
                    /* save */
                    GPS_LOCK {
                        gpsFixUnsafe.pdop  = gpsLastPDOP;
                        gpsFixUnsafe.hdop  = gpsLastHDOP;
                        gpsFixUnsafe.vdop  = gpsLastVDOP;
                        gpsFixUnsafe.nmea |= NMEA0183_GPGSA;
                    } GPS_UNLOCK
                    
                }
#endif
            } else {
                
                // unsupported record type
                
            }

            /* valid fix? */
            // return now if we're concerned about timeouts and we have a valid fix
            if ((timeoutSec > 0L) && validFix_GPRMC) {
                return 1; // ok
            }

        } else {

            // sentence does not start with "$GP..."

        }
             
    } // while - read gps data
    
    return 0; // timeout
    
}

// ----------------------------------------------------------------------------

#if defined(GPS_THREAD)

/* where the thread spends its time */
static void _gpsThreadRunnable(void *arg)
{
    const char *gpsPortName = propGetString(PROP_CFG_GPS_PORT, DEFAULT_GPS_PORT);

    /* simulator loop */
#if defined(GPS_DEVICE_SIMULATOR)
    gpsSimulator = strEqualsIgnoreCase(gpsPortName, GPS_SIMULATOR_PORT);
    if (gpsSimulator) {
        logWARNING(LOGSRC,"Starting GPS simulator thread");
        while (gpsRunThread) {
            _gpsReadGPSFix(0L); // no timeouts
        }
        logERROR(LOGSRC,"Stopping GPS simulator thread");
        threadExit();
        return;
    }
#endif

    /* non-simulator loop */
    TimerSec_t gpsLastOpenErrorTimer = 0L;
    while (gpsRunThread) {
        
        /* power-conservation */
        // The HP hw6945 turns off the GPS receiver when the comport is closed.  To conserve
        // power, we may want to wait here until the next 'gpsAquire(...)' is called.  This is
        // only checked if the GPS sample interval is fairly long (ie. greater than 2 minutes).
        // The trade-off is that with infrequent GPS sampling, arrivals/departures, etc, may
        // be delayed, or may even be missed entirely.
        UInt32 intervalSec        = propGetUInt32(PROP_GPS_SAMPLE_RATE, 15L);
        utBool powerSaveMode      = (intervalSec >= POWER_SAVE_THRESHOLD)? utTrue : utFalse;
        UInt32 powerSaveTimeoutMS = 0L;
        AQUIRE_LOCK {
            if (powerSaveMode) {
                // We're only sampling the GPS every POWER_SAVE_THRESHOLD seconds, or more.
                // Wait here unit 'gpsAquire(...)' tells us to continue
                logDEBUG(LOGSRC,"PowerSave - waiting for 'gpsAquire(...)' ...");
                while (!gpsAquireRequest && gpsRunThread) {
                    // wait for condition (ie. another thread requesting a GPS fix)
                    AQUIRE_WAIT
                }
                powerSaveTimeoutMS = gpsAquireTimeoutMS;
            }