gpstools.c

Open DMT Client C Source code

// ----------------------------------------------------------------------------
// Copyright 2006-2007, Martin D. Flynn
// All rights reserved
// ----------------------------------------------------------------------------
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// 
// http://www.apache.org/licenses/LICENSE-2.0
// 
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// ----------------------------------------------------------------------------
// Description:
//  GPS tools 
//  Tools for managing GPS Latitude/Longitude and calculating distance.
// ---
// General References:
//  http://williams.best.vwh.net/avform.htm
//  http://www.boeing-727.com/Data/fly%20odds/distance.html    [excellent ref]
//  http://williams.best.vwh.net/intersect.htm
//  http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry1
//  http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry3#satellites
//  http://geometryalgorithms.com/Archive/algorithm_0102/algorithm_0102.htm#Distance%20to%20Ray%20or%20Segment
//  http://mathforum.org/library/drmath/sets/select/dm_lat_long.html
//  http://mathforum.org/library/drmath/view/51818.html
// Heading/Bearing:
//  http://mathforum.org/library/drmath/view/55417.html
//  http://williams.best.vwh.net/avform.htm
// Velocities:
//  http://www.xbow.com/Support/Support_pdf_files/NAV420AppNote.pdf
// ---
// Change History:
//  2006/01/04  Martin D. Flynn
//     -Initial release
//  2006/03/12  Martin D. Flynn
//     -Fixed bug where Longitude was not encoded/decoded properly in the Eastern Hemishpere.
//  2006/06/08  Martin D. Flynn
//     -Added 'pdop' to 'GPT_t'
//  2007/01/28  Martin D. Flynn
//     -WindowsCE port
//     -Added support for 'GPSOdometer_t'
// ----------------------------------------------------------------------------

#include "stdafx.h" // TARGET_WINCE
#define SKIP_TRANSPORT_MEDIA_CHECK // only if TRANSPORT_MEDIA not used in this file 
#include "custom/defaults.h"

#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

#include "tools/stdtypes.h"
#include "tools/strtools.h"
#include "tools/gpstools.h"
#include "tools/utctools.h"

#include "custom/log.h"

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

static double SQUARE(double X) { return X * X; }

#if defined(TARGET_WINCE)
#define IS_NAN(X)   _isnan(X)
#define SNPRINTF    _snprintf
#else
#define IS_NAN(X)   isnan(X)
#define SNPRINTF    snprintf
#endif

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

/* clear GPSPoint_t structure */
void gpsPointClear(GPSPoint_t *gp)
{
    gpsPoint(gp, GPS_UNDEFINED_LATITUDE, GPS_UNDEFINED_LONGITUDE);
}

/* init GPSPoint_t structure */
GPSPoint_t *gpsPoint(GPSPoint_t *gp, double lat, double lon)
{
    if (gp) {
        gp->latitude  = lat;
        gp->longitude = lon;
    }
    return gp;
}

/* copy source point to destination structure */
GPSPoint_t *gpsPointCopy(GPSPoint_t *d, const GPSPoint_t *s)
{
    if (d && s) {
        memcpy(d, s, sizeof(GPSPoint_t));
        return d;
    } else {
        return (GPSPoint_t*)0;
    }
}

/* return true if the specified point is valid */
utBool gpsPointIsValid(const GPSPoint_t *gp)
{
    // return 'true' if this is a valid GPS point, 'false' otherwise
    if (gp) {
        //if (isNAN(gp->latitude) || isNAN(gp->longitude)) {
        //    return utFalse;
        //} else
        if ((gp->latitude == 0.0) && (gp->longitude == 0.0)) {
            return utFalse;
        } else
        if ((gp->latitude  >=  90.0) || (gp->latitude  <=  -90.0) ||
            (gp->longitude >= 180.0) || (gp->longitude <= -180.0)   ) {
            return utFalse;
        } else {
            return utTrue;
        }
    } else {
        return utFalse;
    }
}

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

/* return the distance (in radian) between the specified points */
double gpsRadiansToPoint(
    const GPSPoint_t *gpS,            // From point
    const GPSPoint_t *gpE)            // To point
{
    
    /* validate both points */
    if ((gpS->latitude  >=  90.0) || (gpS->latitude  <=  -90.0) ||
        (gpS->longitude >= 180.0) || (gpS->longitude <= -180.0) ||
        (gpE->latitude  >=  90.0) || (gpE->latitude  <=  -90.0) ||
        (gpE->longitude >= 180.0) || (gpE->longitude <= -180.0)   ) {
        return 0.0;
    }

    // Haversine formula (faster and less prone to rounding errors than 'Law of Cosines' algorithm
    // required math functions: sin, cos, atan2, sqrt
    double radLatS, radLonS, radLatE, radLonE, dlat, dlon, a, rad;
    radLatS = gpS->latitude  * RADIANS;  // Y
    radLonS = gpS->longitude * RADIANS;  // X
    radLatE = gpE->latitude  * RADIANS;  // Y
    radLonE = gpE->longitude * RADIANS;  // X
    dlat    = radLatE - radLatS;
    dlon    = radLonE - radLonS;
    a       = SQUARE(sin(dlat/2.0)) + (cos(radLatS) * cos(radLatE) * SQUARE(sin(dlon/2.0)));
    rad     = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
    return rad;
    
}

/* return the distance (in meters) between the specified points */
double gpsMetersToPoint(
    const GPSPoint_t *gpS,            // From point
    const GPSPoint_t *gpE)            // To point
{
    double rad = gpsRadiansToPoint(gpS, gpE);
    return rad * EARTH_RADIUS_METERS;
}

/* return the distance (in kilometers) between the specified points */
double gpsKilometersToPoint(
    const GPSPoint_t *gpS,            // From point
    const GPSPoint_t *gpE)            // To point
{
    double rad = gpsRadiansToPoint(gpS, gpE);
    return (rad * EARTH_RADIUS_METERS) / 1000.0;
}
// ----------------------------------------------------------------------------

/* copy source GPS structure to destination */
GPS_t *gpsCopy(GPS_t *dest, const GPS_t *src)
{
    if (dest) {
        if (src) {
            memcpy(dest, src, sizeof(GPS_t));
        } else {
            gpsClear(dest);
        }
        return dest;
    } else {
        return (GPS_t*)0;
    }
}

/* initialize GPS structure */
GPS_t *gpsClear(GPS_t *gps)
{
    if (gps) {
        memset(gps, 0, sizeof(GPS_t));
        // explicitly init all floating point values ...
        gps->point.latitude  = GPS_UNDEFINED_LATITUDE;
        gps->point.longitude = GPS_UNDEFINED_LONGITUDE;
        gps->accuracy        = GPS_UNDEFINED_ACCURACY;
        gps->speedKPH        = GPS_UNDEFINED_SPEED;
        gps->heading         = GPS_UNDEFINED_HEADING;
        gps->altitude        = GPS_UNDEFINED_ALTITUDE;
        gps->pdop            = GPS_UNDEFINED_DOP;
        gps->hdop            = GPS_UNDEFINED_DOP;
        gps->vdop            = GPS_UNDEFINED_DOP;
    }
    return gps;
}

/* return true if the specified GPS stgructure has been populated with a valid GPS fix */
utBool gpsIsValid(const GPS_t *gps)
{
    if (!gps) {
        // no GPS specified
        return utFalse;
    } else
    if (gps->fixtime <= 0L) {
        // no valid GPS fix
        return utFalse;
    } else {
        return utTrue;
    }
}

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

/* parse strng into GPS structure */
GPS_t *gpsParseString(GPS_t *gps, const char *s)
{
    // <fixtime>,<latitude>,<longitude>,<accuracy>,<speed>,<heading>,<altitude>
    if (!gps || !s) {
        
        // nothing to parse
        return (GPS_t*)0;
        
    } else {
        
        gpsClear(gps);
        int f;
        for (f = 0; f < 7; f++) { // 7 possible fields
            
            /* skip to field */
            while (*s && isspace(*s)) { s++; }
            if (!*s || (!isdigit(*s) && (*s != '-'))) { break; }
            
            /* parse field */
            switch (f) {
                case 0: gps->fixtime         =        strParseUInt32(s,      0L); break;
                case 1: gps->point.latitude  =        strParseDouble(s,     0.0); break;
                case 2: gps->point.longitude =        strParseDouble(s,     0.0); break;
                case 3: gps->accuracy        = (float)strParseDouble(s,     0.0); break;
                case 4: gps->speedKPH        = (float)strParseDouble(s,    -1.0); break;
                case 5: gps->heading         = (float)strParseDouble(s,    -1.0); break;
                case 6: gps->altitude        = (float)strParseDouble(s, -9999.0); break;
            }

            /* skip over comma */
            while (*s && (*s != ',')) { s++; }
            if (*s != ',') { break; }
            *s++;

        }
        return gps;
        
    }
}

/* create a string from the specified GPS structure */
const char *gpsToString(GPS_t *gps, utBool all, char *s, int slen)
{
    // <fixtime>,<latitude>,<longitude>,<accuracy>,<speed>,<heading>,<altitude>
    if (s && (slen > 0)) {
        if (all) {
            SNPRINTF(s, slen, "%lu,%.6lf,%.6lf,%.1f,%.1f,%.1f,%.1f", 
                gps->fixtime,
                gps->point.latitude, gps->point.longitude,
                gps->accuracy,
                gps->speedKPH,
                gps->heading,
                gps->altitude
                );
        } else {
            SNPRINTF(s, slen, "%lu,%.6lf,%.6lf", 
                gps->fixtime,
                gps->point.latitude, gps->point.longitude
                );
        }
        return s;
    }
    return (char*)0;
}

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

/* parse GPSOdometer_t from string */
GPSOdometer_t *gpsOdomParseString(GPSOdometer_t *gps, const char *s)
{
    // <fixtime>,<latitude>,<longitude>,<meters>
    if (!gps || !s) {
        
        // nothing to parse
        return (GPSOdometer_t*)0;
        
    } else {
        
        memset(gps, 0, sizeof(GPSOdometer_t));
        gpsPointClear(&(gps->point));
        int f;
        for (f = 0; f < 4; f++) { // 4 possible fields
            
            /* skip to field */
            while (*s && isspace(*s)) { s++; }
            if (!*s || (!isdigit(*s) && (*s != '-'))) { break; }
            
            /* parse field */
            switch (f) {
                case 0: gps->fixtime         = strParseUInt32(s,  0L); break;
                case 1: gps->point.latitude  = strParseDouble(s, 0.0); break;
                case 2: gps->point.longitude = strParseDouble(s, 0.0); break;
                case 3: gps->meters          = strParseUInt32(s,  0L); break;
            }

            /* skip over comma */
            while (*s && (*s != ',')) { s++; }
            if (*s != ',') { break; }
            *s++;

        }
        return gps;

    }
}

/* print GPSOdometer_t to string */
const char *gpsOdomToString(GPSOdometer_t *gps, char *s, int slen)
{
    // <fixtime>,<latitude>,<longitude>,<meters>
    if (s && (slen > 0)) {
        SNPRINTF(s, slen, "%lu,%.6lf,%.6lf,%lu", 
            gps->fixtime,
            gps->point.latitude, gps->point.longitude,
            gps->meters
            );
        return s;
    }
    return (char*)0;
}

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

/* encode GPS lat/lon to 6-byte buffer */
UInt8 *gpsPointEncode6(UInt8 *buf, const GPSPoint_t *gps)
{
    if (gpsPointIsValid(gps)) {
        int i;
        UInt32 rLat = (UInt32)((gps->latitude  -  90.0) * (POW2_24F / -180.0));
        UInt32 rLon = (UInt32)((gps->longitude + 180.0) * (POW2_24F /  360.0)); // BUGFIX_2006/03/12
        for (i = 2; i >= 0; i--) {
            buf[i    ] = (UInt8)(rLat & 0xFF);
            buf[i + 3] = (UInt8)(rLon & 0xFF);
            rLat >>= 8;
            rLon >>= 8;
        }
    } else {
        memset(buf, 0, 6);
    }
    return buf;
}

/* decode GPS lat/lon from 6-byte buffer */
GPSPoint_t *gpsPointDecode6(GPSPoint_t *gps, const UInt8 *buf)
{
    int i;
    UInt32 rLat = 0L, rLon = 0L;
    for (i = 0; i < 3; i++) {
        rLat = (rLat << 8) | buf[i    ];
        rLon = (rLon << 8) | buf[i + 3];
    }
    if (rLat || rLon) {
        gps->latitude  = ((double)rLat * (-180.0 / POW2_24F)) +  90.0;
        gps->longitude = ((double)rLon * ( 360.0 / POW2_24F)) - 180.0; // BUGFIX_2006/03/12
    } else {
        gps->latitude  = 0.0;
        gps->longitude = 0.0;
    }
    return gps;
}

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

/* encode GPS lat/lon to 8-byte buffer */
UInt8 *gpsPointEncode8(UInt8 *buf, const GPSPoint_t *gps)
{
    if (gpsPointIsValid(gps)) {
        int i;
        UInt32 rLat = (UInt32)((gps->latitude  -  90.0) * (POW2_32F / -180.0));
        UInt32 rLon = (UInt32)((gps->longitude + 180.0) * (POW2_32F /  360.0)); // BUGFIX_2006/03/12
        for (i = 3; i >= 0; i--) {
            buf[i    ] = (UInt8)(rLat & 0xFF);
            buf[i + 4] = (UInt8)(rLon & 0xFF);
            rLat >>= 8;
            rLon >>= 8;
        }
    } else {
        memset(buf, 0, 8);
    }
    return buf;
}

/* decode GPS lat/lon from 8-byte buffer */
GPSPoint_t *gpsPointDecode8(GPSPoint_t *gps, const UInt8 *buf)
{
    int i;
    UInt32 rLat = 0L, rLon = 0L;
    for (i = 0; i < 4; i++) {
        rLat = (rLat << 8) | buf[i    ];
        rLon = (rLon << 8) | buf[i + 4];
    }
    if (rLat || rLon) {
        gps->latitude  = ((double)rLat * (-180.0 / POW2_32F)) +  90.0;
        gps->longitude = ((double)rLon * ( 360.0 / POW2_32F)) - 180.0; // BUGFIX_2006/03/12
    } else {
        gps->latitude  = 0.0;
        gps->longitude = 0.0;
    }
    return gps;
}

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