// ----------------------------------------------------------------------------// 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 fix checking for odometer events.//  Examines changes in GPS fix data and generates odometer events accordingly.// ---// Change History://  2006/01/04  Martin D. Flynn//     -Initial release//  2007/01/28  Martin D. Flynn//     -WindowsCE port//     -This module has been updated to accomodate the odometer resolution change//      from 0.1 meter unit to 1 meter units.// ----------------------------------------------------------------------------#include "stdafx.h" // TARGET_WINCE#include "custom/defaults.h"#include <stdio.h>#include <stdlib.h>#include <string.h>#include <ctype.h>#include "custom/log.h"#include "custom/gps.h"#include "tools/stdtypes.h"#include "tools/gpstools.h"#include "tools/utctools.h"#include "base/propman.h"#include "base/statcode.h"#include "base/event.h"#include "modules/odometer.h"// ----------------------------------------------------------------------------#define ODOMETER_PRIORITY   PRIORITY_HIGHstatic Int32 _round(double D) { return (D>=0.0)?(Int32)(D+0.5):(Int32)(D-0.5); }#define ROUND(D) _round(D)   // ((UInt32)((D) + 0.5))// ----------------------------------------------------------------------------static eventAddFtn_t  ftnQueueEvent = 0;static struct {    Key_t           value;    Key_t           limit;    Key_t           state;    StatusCode_t    code;} odomTable[] = {   // Meters                 Alarm Limit            Odom State/GPS       Status Code    { PROP_ODOMETER_0_VALUE, PROP_ODOMETER_0_LIMIT, PROP_ODOMETER_0_GPS, STATUS_ODOM_LIMIT_0 },    { PROP_ODOMETER_1_VALUE, PROP_ODOMETER_1_LIMIT, PROP_ODOMETER_1_GPS, STATUS_ODOM_LIMIT_1 },    { PROP_ODOMETER_2_VALUE, PROP_ODOMETER_2_LIMIT, PROP_ODOMETER_2_GPS, STATUS_ODOM_LIMIT_2 },    { PROP_ODOMETER_3_VALUE, PROP_ODOMETER_3_LIMIT, PROP_ODOMETER_3_GPS, STATUS_ODOM_LIMIT_3 },    { PROP_ODOMETER_4_VALUE, PROP_ODOMETER_4_LIMIT, PROP_ODOMETER_4_GPS, STATUS_ODOM_LIMIT_4 },    { PROP_ODOMETER_5_VALUE, PROP_ODOMETER_5_LIMIT, PROP_ODOMETER_5_GPS, STATUS_ODOM_LIMIT_5 },    { PROP_ODOMETER_6_VALUE, PROP_ODOMETER_6_LIMIT, PROP_ODOMETER_6_GPS, STATUS_ODOM_LIMIT_6 },    { PROP_ODOMETER_7_VALUE, PROP_ODOMETER_7_LIMIT, PROP_ODOMETER_7_GPS, STATUS_ODOM_LIMIT_7 }};#define ODOMETER_COUNT          (sizeof(odomTable)/sizeof(odomTable[0]))// ----------------------------------------------------------------------------static UInt8 odomFirstInit[ODOMETER_COUNT];#define ODOM_IsFirst(X)         (odomFirstInit[(X)])#define ODOM_SetFirst(X,V)      {odomFirstInit[(X)] = (V);}/* get odometer state for specified index */static GPSOdometer_t *_odomGetState(UInt16 ndx){    GPSOdometer_t *gps = (GPSOdometer_t*)0;    if (ndx < ODOMETER_COUNT) {        gps = (GPSOdometer_t*)propGetGPS(odomTable[ndx].state, (GPSOdometer_t*)0);    }    if (!gps) {        logCRITICAL(LOGSRC,"Internal odometer error");    }    return gps;}// ----------------------------------------------------------------------------/* initialize odometer */void odomInitialize(eventAddFtn_t queueEvent){    ftnQueueEvent = queueEvent;    memset(odomFirstInit, 0, sizeof(odomFirstInit));}// ----------------------------------------------------------------------------/* return actual odometer value for the vehicle to which the device is attached */// will return '0' if the actual odometer is not availabledouble odomGetActualOdometerMeters(){    // implement 'actual' odometer value retrieval here (via OBC, etc).    return 0.0;}/* return meters value at specified index */double odomGetDistanceMetersAtIndex(int ndx){    if (ndx == 0) {        // vehicle distance (may need special handling)        return (double)propGetUInt32(odomTable[0].value, 0L);    } else    if ((ndx > 0) && (ndx < ODOMETER_COUNT)) {        // driver, etc        return (double)propGetUInt32(odomTable[ndx].value, 0L);    } else {        // invalid index        return 0.0;    }}/* return total meters for device */double odomGetDeviceDistanceMeters(){    // vehicle distance (may need special handling)    return odomGetDistanceMetersAtIndex(0);}/* return meters value at specified index */utBool odomResetDistanceMetersAtIndex(int ndx){    if ((ndx >= 0) && (ndx < ODOMETER_COUNT)) {        propSetUInt32(odomTable[ndx].value, 0L);        return utTrue;    } else {        return utFalse;    }}// ----------------------------------------------------------------------------/* queue new event */static void _queueOdometerEvent(PacketPriority_t priority, StatusCode_t code, const GPS_t *gps, UInt32 odomMeters){        /* make sure we have a GPS fix */    GPS_t gpsFix;    if (!gps) {        // all odometers events should have a GPS fix, so this should never occur        gps = gpsGetLastGPS(&gpsFix, 0);    }    /* queue event */    if (gps && ftnQueueEvent) {        Event_t evRcd;        evSetEventDefaults(&evRcd, code, 0L, gps);        evRcd.distanceKM = (double)odomMeters / 1000.0;        (*ftnQueueEvent)(priority, DEFAULT_EVENT_FORMAT, &evRcd);    }    }// ----------------------------------------------------------------------------/* periodic check new GPS fix */void odomCheckGPS(const GPS_t *oldFix, const GPS_t *newFix){    // 'newFix' may be NULL    /* get actual odometer, if available */    UInt32 actualOdomMeters = ROUND(odomGetActualOdometerMeters());    /* loop through odometers */    UInt32 minDeltaMeters = propGetUInt32(PROP_GPS_DISTANCE_DELTA, 500L);        if (minDeltaMeters < 10L) { minDeltaMeters = 10L; }    int i;    for (i = 0; i < ODOMETER_COUNT; i++) {        GPSOdometer_t *gps = _odomGetState(i);        if (!gps) { continue; } // error (invalid index)                /* update odometer value */        UInt32 oldOdomMeters = propGetUInt32(odomTable[i].value, 0L);        UInt32 newOdomMeters = 0L;        if ((!oldOdomMeters && !ODOM_IsFirst(i)) || // <-- I've reset the value            (!gps->fixtime || !gpsPointIsValid(&(gps->point)))) {            // First tripometer check (after a reset)            if (i == 0) {                // vehicle odometer                newOdomMeters = (actualOdomMeters > 0L)? actualOdomMeters : oldOdomMeters;                gps->meters = 0L; // vehicle odometer base is '0'            } else {                // tripometer                newOdomMeters = 0L;                gps->meters = ROUND(odomGetDeviceDistanceMeters());            }            propSetUInt32(odomTable[i].value, newOdomMeters);            if (newFix) {                memcpy((GPSShort_t*)gps, (GPSShort_t*)newFix, sizeof(GPSShort_t));            }            propSetGPS(odomTable[i].state, gps);            ODOM_SetFirst(i,1); // this will be the first fix        } else        if (newFix) {            // GPS based odometer            UInt32 deltaMeters = ROUND(gpsMetersToPoint(&(newFix->point), &(gps->point)));            if (deltaMeters >= minDeltaMeters) {                // I've moved at least the minimum required distance to set a new steak in the ground                newOdomMeters = ODOM_IsFirst(i)? deltaMeters : (deltaMeters + oldOdomMeters);                propSetUInt32(odomTable[i].value, newOdomMeters);                memcpy((GPSShort_t*)gps, (GPSShort_t*)newFix, sizeof(GPSShort_t));                propSetGPS(odomTable[i].state, gps);                ODOM_SetFirst(i,0); // no longer the first fix            }        }                /* check odometer limits */        // only sends an event at the crossing of the limit        if (newOdomMeters > 0L) {            UInt32 limitOdomMeters = propGetUInt32(odomTable[i].limit, 0L);            if ((limitOdomMeters > oldOdomMeters) && (limitOdomMeters <= newOdomMeters)) {                StatusCode_t code = (StatusCode_t)odomTable[i].code;                _queueOdometerEvent(ODOMETER_PRIORITY, code, newFix, newOdomMeters);            }        }            }    }// ----------------------------------------------------------------------------