head     4.4;
access   ;
symbols  ;
locks    oasisa:4.4; strict;
comment  @ * @;


4.4
date     2001.06.19.12.13.54;  author oasisa;  state Exp;
branches ;
next     ;


desc
@New Repository; 6/19/2001 (klh)
@



4.4
log
@New Repository; 6/19/2001 (klh)
@
text
@/****************************************************************************/
/* Copyright 1991-1995 MBARI						    */
/****************************************************************************/
/* $Header: gps10.c,v 1.1 2001/06/19 11:43:40 oasisa Exp $			    */
/* Summary  : Driver Routines for 10 Channel Magellan GPS		    */
/* Filename : gps10.c							    */
/* Author   : Robert Herlien (rah)					    */
/* Project  : OASIS Mooring						    */
/* $Revision: 1.1 $							    */
/* Created  : 02/27/97							    */
/*									    */
/* MBARI provides this documentation and code "as is", with no warranty,    */
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium  */
/* Research Institute to assist in its use, correction, modification, or    */
/* enhancement. This information should not be published or distributed to  */
/* third parties without specific written permission from MBARI.            */
/*									    */
/****************************************************************************/
/* Modification History:						    */
/* 27feb95 rah - created from gps.c, the 5 channel Magellan driver	    */
/* $Log:	gps10.c,v $
 * Revision 1.1  2001/06/19  11:43:40  11:43:40  oasisa (Oasis users)
 * Initial revision
 * 
 * Revision 4.3  99/06/16  10:21:36  10:21:36  bobh (Bob Herlien)
 * Mar/May '99 Deployments of M3/M2
 * 
 * Revision 4.2  98/09/09  10:48:06  10:48:06  bobh (Bob Herlien)
 * Sept/Oct '98 deployments of M1, Eqpac 1 & 2
 * 
 * Revision 4.1  98/05/12  09:35:12  09:35:12  bobh (Bob Herlien)
 * June '98 turnaround for EqPac
 * 
 * Revision 4.0  98/03/09  11:44:44  11:44:44  bobh (Bob Herlien)
 * M3 Deployment of March '98, new Sat-Pac driver
 * 
 * Revision 3.8  97/09/12  10:50:57  10:50:57  bobh (Bob Herlien)
 * Redeploy M1
 * 
 * Revision 3.7  97/07/23  11:18:16  11:18:16  bobh (Bob Herlien)
 * July '97 M1 deployment, new shutter code
 * 
 * Revision 3.5  96/07/17  13:01:35  13:01:35  bobh (Bob Herlien)
 * July '96 deployment of M2 with ARGOS code
 * 
 * Revision 3.4  96/06/18  15:24:28  15:24:28  bobh (Bob Herlien)
 * June '96 deployment of M1
 * 
 * Revision 3.3  95/04/13  13:47:01  13:47:01  hebo (Bob Herlien)
 * Drifter Deployment for Coop (flip) cruise
 * 
 * Revision 3.2  95/04/11  14:03:30  14:03:30  hebo (Bob Herlien)
 * Drifter Deployment on IronEx
 * 
*/
/****************************************************************************/

#include <types.h>			/* MBARI type definitions	    */
#include <const.h>			/* MBARI constants		    */
#include <oasis.h>			/* OASIS controller definitions	    */
#include <task.h>			/* OASIS Multitasking definitions   */
#include <gps10.h>			/* 10 channel Magellan definitions  */
#include <io.h>				/* OASIS I/O definitions	    */
#include <log.h>			/* Log record definitions	    */
#include <custom.h>			/* GPS_BIT definition		    */
#include <limits.h>			/* Ranges for type definitions	    */
#include <stdio.h>			/* Standard I/O functions	    */

#pragma(noreentrant)			/* This module non-reentrant	    */

#ifndef GPS_LAT
#define GPS_LAT		3645
#endif
#ifndef GPS_LONG
#define GPS_LONG	-12150
#endif


/********************************/
/*	External Functions	*/
/********************************/

Extern char	*drvSerPortAndMalloc( Driver *dp, Nat16 size );
Extern Void	drvSerReleaseAndFree( Driver *dp, char *buffer );
Extern Void	drv_pwron( Driver *dp );
Extern Void	drv_pwroff( Driver *dp );
Extern Void	drvLog( Driver *dp, Byte *samplep, Int16 len );
Extern Void	drvLogError( Driver *dp, Errno err );
Extern Void	doputc( Int16 c );
Extern Void	xputs( const char *s );
Extern Int16	xgetc_tmout( Nat16 tmout  );
Extern Int16	xgetn_tmout( char *s, Int16 len, Nat16 tmout );
Extern Int16	swap( Int16 w );
Extern Nat32	swaplong( Nat32 lwrd );
Extern Void	bcopy( const Byte *src, Byte *dst, Nat16 len );
Extern Void	bcopyBackwards( const Byte *src, Byte *dst, Nat16 len );
Extern Void	bzero( void *s, int n );
Extern Void	tmpFree( char *ptr );
Extern MBool	clk_read( Void );
Extern TimeOfDay dtToTod( Reg DateTime *dtp );
Extern Void	drvr_timechange( Int32 diff );


/********************************/
/*	External Function	*/
/********************************/

Extern Nat32	compute_one_err10( GpsSample *samplep, GpsRec *gpsrp );


/********************************/
/*	External Data		*/
/********************************/

Extern Reg Byte		io_wakebits;	/* Copy of wakeup bits		    */
Extern Reg TimeOfDay	tod;		/* Current time in TimeOfDay format */
Extern Reg DateTime	dttm;		/* Current date & time in DateTime  */


/********************************/
/*	Global Data		*/
/********************************/

#ifdef GPS_TYPE
#if ( GPS_TYPE >= 10 )		/* Only in "full-feature" driver	    */
				/* Following set by user via "parms"	    */
Global Int16		gpsInitLat;	/* Initial GPS latitude		    */
Global Int16		gpsInitLong;	/* Initial GPS longitude	    */
Global Int16		gpsInitAlt;	/* Initial GPS altitude		    */
Global Int16		gpsElMask;	/* GPS elevation mask		    */
Global Int16		gpsVelAvg;	/* GPS velocity averaging time	    */
#endif
#endif

Global Int16		gpsAlmanacAge;	/* GPS almanac age		    */

#ifdef ALLOW_PERM_WAKE
Global MBool            GPS_KEEPALIVE;
#endif

/********************************/
/*	Module Local Data	*/
/********************************/

const GpsSetupMsg gpsSetupDefault = 
{1, 1, {1, 0, 0, 1}, 0x2003, 10, 0, 0, -32, 0, 0, 1, 0, 1, 0x14, 0, '\n'};

/* Note that 0x2003 (PDOS) is swap(800);  local time offset is -8 hrs */


/********************************/
/*	Forward Declarations	*/
/********************************/

Int16	get_fix( Driver *dp, GpsSample *sample, GpsMsg *gpsmsgp );
Word	get_status( Driver *dp, GpsMsg *gpsmsgp );
Void	get_almanac( Driver *dp, GpsMsg *gpsmsgp );
Void	compute_values( Driver *dp, GpsSample *samplep, GpsRec *gpsrp );


/************************************************************************/
/* Function    : gps_pwron						*/
/* Purpose     : Turn on GPS power					*/
/* Inputs      : Driver ptr						*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gps_pwron( Driver *dp )
{
    drv_pwron( dp );			/* Turn on main power		*/
    task_delay( TICKS_PER_SECOND/2 );	/* Wait for power to stabilize	*/
    io_wakebits |= GPS_BIT;		/* Turn on power control bit	*/
    wakeport = io_wakebits;		/* Send local copy to hardware	*/
    task_delay( TICKS_PER_SECOND/2 );	/* Wait for board to init	*/

} /* gps_pwron() */


/************************************************************************/
/* Function    : gps_pwroff						*/
/* Purpose     : Turn off GPS power					*/
/* Inputs      : Driver ptr						*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gps_pwroff( Driver *dp )
{
#ifdef ALLOW_PERM_WAKE
  if(GPS_KEEPALIVE==FALSE){
    task_delay( TICKS_PER_SECOND/2 );	/* Wait for any serial chars to send*/
    io_wakebits &= ~GPS_BIT;		/* Turn off power control bit	*/
    wakeport = io_wakebits;		/* Send local copy to hardware	*/
  }
#else
    task_delay( TICKS_PER_SECOND/2 );	/* Wait for any serial chars to send*/
    io_wakebits &= ~GPS_BIT;		/* Turn off power control bit	*/
    wakeport = io_wakebits;		/* Send local copy to hardware	*/
#endif

   task_delay( TICKS_PER_SECOND/4 );	/* Wait for GPS board		*/
    drv_pwroff( dp );			/* Turn off main power		*/

} /* gps_pwroff() */


/************************************************************************/
/* Function    : sendGpsMsg						*/
/* Purpose     : Send a binary message to Magellan receiver		*/
/* Inputs      : Ptr to message content, length				*/
/* Outputs     : None							*/
/************************************************************************/
	Void
sendGpsMsg( GpsMsg *msgp, Int16 len )
{
    Byte	chksum;
    Byte	*p;
    Int16	i;

    xputs("$$");
    chksum = (len >> 8);
    doputc( chksum );
    chksum ^= len;
    doputc( len & 0xff );

    for( i = 0, p = (Byte *)msgp; i < len - 2; i++, p++ )
    {
	doputc( *p );
	chksum ^= *p;
    }

    doputc( chksum );
    doputc( '\n' );

} /* sendGpsMsg() */


/************************************************************************/
/* Function    : getGpsMsg						*/
/* Purpose     : Get a binary message from Magellan receiver		*/
/* Inputs      : Ptr to message buffer, timeout				*/
/* Outputs     : Message length, exclusive of header, or ERROR		*/
/************************************************************************/
	Int16
getGpsMsg( GpsMsg *msgp, Nat16 tmout )
{
    Reg Int16	c;
    Int16	len;
    Reg Byte	chksum;
    Reg Byte	*p;

    while ( (c = xgetc_tmout(tmout)) != '$' )
	if ( c == ERROR )
	    return( ERROR );

    if ( xgetc_tmout(tmout) != '$' )
	return( ERROR );

    if ( xgetn_tmout((char *)(&len), sizeof(len), tmout) != sizeof(len) )
	return( ERROR );

    len = swap( len );

    if ( len > GPS_BUFSIZE )
	return( ERROR );

    if ( xgetn_tmout((char *)msgp, len, tmout) < len )
	return( ERROR );

    chksum = (len >> 8) ^ (len & 0xff);

    for( c = 0, p = (Byte *)msgp; c < len - 2; c++ )
	chksum ^= *p++;

    if ( chksum != *p++ )
	return( ERROR );

    return( len );

} /* getGpsMsg() */


/************************************************************************/
/* Function    : getAndCheckGpsMsg					*/
/* Purpose     : Get a GPS message, check for correct format for our use*/
/* Inputs      : Driver pointer, Ptr to message buffer			*/
/* Outputs     : Valid Message type or Error code (see include file)	*/
/************************************************************************/
	Int16
getAndCheckGpsMsg( Driver *dp, GpsMsg *msgp )
{
    Reg Int16	len;

    if ( (len = getGpsMsg(msgp, dp->drv_parms[TIMEOUT])) == ERROR )
	return( TMOUT_ERR );

    switch( msgp->positionMsg.posType )
    {
      case POSITION_MSG:
	if ( (len < (sizeof(GpsPosition) - 2)) ||
	     (msgp->positionMsg.posSubtype > 0x34) )
	    return( BAD_DATA );

/* Grungy fix because position message has odd alignment	*/
/* We put in a pad byte in the msg struct, and now need to copy down*/
/* This is also the reason for the "-2" in length compare above	*/	    
	bcopyBackwards( (Byte *)(&msgp->positionMsg.posPad),
			(Byte *)(&msgp->positionMsg.posTime),
			sizeof(GpsPosition) - 4 );

	if ( msgp->positionMsg.posStatus & 3 )	/* If too old or dead reckon*/
	    return( NOT_AVAIL_ERR );		/*  reject it		    */

	return( POSITION_MSG );


      case TIME_MSG:
	if ( (len < sizeof(GpsTime)) || (msgp->timeMsg.timeSubtype != 1) )
	    return( BAD_DATA );

	return( TIME_MSG );


      case SAT_STATUS_MSG:
	if ( len < sizeof(GpsSatStatus) )
	    return( BAD_DATA );

	return( SAT_STATUS_MSG );


      case GENERAL_STATUS_MSG:
	if ( len < sizeof(GpsGeneralStatus) )
	    return( BAD_DATA );

	if ( (msgp->generalStatus.rcvrMode == TRACKING2D) ||
	     (msgp->generalStatus.rcvrMode == TRACKING3D) )
	    dp->drv_flags |= GPS_TRACKING;	/* Show we're getting fixes*/
	else
	    dp->drv_flags &= ~GPS_TRACKING;	/* We're not getting fixes */

	return( GENERAL_STATUS_MSG );

    }

    return( BAD_DATA );

} /* getAndCheckGpsMsg() */


/************************************************************************/
/* Function    : gpsSetup						*/
/* Purpose     : Send a Setup (type 1) message to Magellan GPS receiver	*/
/* Inputs      : Message subtype, msg rates for type 81, 82, 83, 84 msgs*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gpsSetup( Byte subtype, Byte rate81, Byte rate82, Byte rate83, Byte rate84 )
{
    GpsSetupMsg		msg;

    bcopy( (const Byte *)(&gpsSetupDefault), (Byte *)(&msg),
	   sizeof(GpsSetupMsg) );

    msg.setupSubtype = subtype;
    msg.msgRates[0] = rate81;
    msg.msgRates[1] = rate82;
    msg.msgRates[2] = rate83;
    msg.msgRates[3] = rate84;
#if ( GPS_TYPE >= 10 )
    msg.maskAngle = gpsElMask;
    msg.velAvgTime = gpsVelAvg;
#endif
    sendGpsMsg( (GpsMsg *)(&msg), sizeof(msg) );

} /* gpsSetup() */


/************************************************************************/
/* Function    : oasisToMagellanPosition				*/
/* Purpose     : Convert dddmm (Int16)  to Magellan 10^-7 degrees (Int32)*/
/* Inputs      : Position in dddmm (degrees, minutes)			*/
/* Outputs     : Position in 10^-7 degrees				*/
/************************************************************************/
	Int32
oasisToMagellanPosition( Int16 pos )
{
    return( swaplong((10000000L * (pos/100)) + (500000L * (pos % 100) / 3)) );

} /* oasisToMagellanPosition() */


/************************************************************************/
/* Function    : gpsInit						*/
/* Purpose     : Initialize Magellan GPS receiver			*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gpsInit( Void )
{
    GpsInitPosition	posMsg;
    GpsInitTime		timeMsg;
    Reg Nat16		year;

    posMsg.initType = INIT_POSITION_MSG; /* Send initial position	*/
    posMsg.initSubtype = 0;
#if ( GPS_TYPE >= 10 )
    posMsg.initLat = oasisToMagellanPosition( gpsInitLat );
    posMsg.initLong = oasisToMagellanPosition( gpsInitLong );
    posMsg.initAlt = swaplong( (Int32)gpsInitAlt );
#else
    posMsg.initLat = oasisToMagellanPosition( GPS_LAT );
    posMsg.initLong = oasisToMagellanPosition( GPS_LONG );
    posMsg.initAlt = 0L;
#endif
    sendGpsMsg( (GpsMsg *)(&posMsg), sizeof(posMsg) );
    task_delay( TICKS_PER_SECOND );

    timeMsg.timeType = INIT_TIME_MSG;	/* Send initial time		*/
    timeMsg.timeSubtype = 0;
    year = dttm.dt_yr;
    year += (year < 70) ? 2000 : 1900;
    timeMsg.initYear = swap( year );
    timeMsg.initMonth = dttm.dt_mo;
    timeMsg.initDay = dttm.dt_day;
    timeMsg.initHour = dttm.dt_hr;
    timeMsg.initMinute = dttm.dt_min;
    timeMsg.initSecond = dttm.dt_sec;
    timeMsg.initFiller = 0;
    sendGpsMsg( (GpsMsg *)(&timeMsg), sizeof(timeMsg) );
    task_delay( TICKS_PER_SECOND );

    gpsSetup( 0, 1, 0, 0, 1 );		/* Type 81 & 84 msgs, save setup*/

} /* gpsInit() */


/************************************************************************/
/* Function    : gps10_wake						*/
/* Purpose     : GPS serial wakeup function, executed when user CONNECTs*/ 
/* Inputs      : Driver ptr, MBool (TRUE for on, FALSE for off)		*/
/* Outputs     : TRUE if OK to use GPS, FALSE if not			*/
/************************************************************************/
	MBool
gps10_wake( Driver *dp, MBool on )
{
    if ( on )
    {				/* Connectint to GPS			*/
	if ( dp->drv_flags & GPS_IN_USE )
	    return( FALSE );	/* If already in use, return FALSE	*/

	gps_pwron( dp );	 /* Turn on GPS power			*/
	gpsSetup(1, 0, 0, 0, 0); /* Turn off GPS messages		*/
    }
    else
	gps_pwroff( dp );	/* If done, turn off GPS power		*/

    return( TRUE );

} /* gps10_wake() */


/************************************************************************/
/* Function    : gps10_drv						*/
/* Purpose     : GPS driver						*/
/* Inputs      : Driver Pointer						*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gps10_drv( Driver *dp )
{
    Int16	samples, fix;		/* Num samples we want, sample num*/
    Nat16	size;			/* Size of buffers		  */
    Errno	rtn;			/* Error return code		  */
    TimeOfDay	start_time;		/* Time we started sampling	  */
    GpsSample	*samplep;		/* Sample buffer		  */
    GpsMsg	*gpsmsgp;		/* Buffer for messages	    	  */
    GpsRec	gpsRec;			/* Record to log		  */

    samples = dp->drv_parms[PARM0];	/* Get number samples to take	  */
    if ( samples > GPS_SAMPLES )	/* Don't exceed buffer size	  */
	    samples = GPS_SAMPLES;

    size = samples * sizeof(GpsSample);

    if ( (gpsmsgp = (GpsMsg *)drvSerPortAndMalloc(dp, size + GPS_BUFSIZE))
	 == (GpsMsg *)NULL )
	return;				/* Get message and sample buffers */

    bzero( (void *)gpsmsgp, size );
    samplep = (GpsSample *)((Byte *)gpsmsgp + GPS_BUFSIZE);

    dp->drv_flags |= GPS_IN_USE;	/* Show we're using GPS		  */
    dp->drv_flags &= ~GPS_TRACKING;	/* Show we're not yet getting fixes*/
    gps_pwron( dp );			/* Turn on GPS receiver		  */

    if ( dp->drv_flags & DO_INIT )	/* If got userif message to init  */
	gpsInit();			/*  initialize GPS receiver	  */
    else
        gpsSetup( 1, 1, 0, 0, 1);	/* Get type 81 & 84 msgs 1/second */

#ifdef ALLOW_PERM_WAKE
    if ( dp->drv_flags & DO_AUX ){	/* If got userif message to init  */
      GPS_KEEPALIVE=(GPS_KEEPALIVE==TRUE?FALSE:TRUE);
      gps_pwron(dp);
      drvSerReleaseAndFree( dp, (char *)gpsmsgp );
      gps_pwroff( dp );			/* Turn off GPS power		  */
      dp->drv_flags &= ~(GPS_IN_USE | GPS_TRACKING | DO_INIT | DO_AUX);
      return;
    }
#endif

    for ( start_time = tod;
          (((tod - start_time) < dp->drv_parms[PARM1]) && 
	  ((dp->drv_flags & GPS_TRACKING) == 0)); )
    {
	rtn = getAndCheckGpsMsg( dp, gpsmsgp ); /* Get 1st fix from GPS rcvr*/
	task_delay( TICKS_PER_SECOND/5 ); /* Let other tasks run	  */
    }

    if ( dp->drv_flags & GPS_TRACKING )	/* If getting GPS fixes		  */
    {
	for ( fix = 0; fix < samples; fix++ )
	{
	    if ( get_fix(dp, samplep + fix, gpsmsgp) < 0 )
		break;			 /* Get the samples, quit if time out*/
	    task_delay( TICKS_PER_SECOND/5 );	/* Let other tasks run	  */
	}

	gpsRec.gps_samples = fix;
	compute_values(dp, samplep, &gpsRec); /* Compute mean, errors	  */
	gpsRec.gps_status = get_status( dp, gpsmsgp );
	gpsRec.gps_ontime = tod - start_time;
	drvLog( dp, (Byte *)&gpsRec, sizeof(gpsRec) );

	if ( dp->drv_flags & DO_INIT )	/* If initializing		  */
	    get_almanac( dp, gpsmsgp );	/*   get current almanac	  */
    }
    else				/* If didn't get first fix,	  */
	drvLogError( dp, rtn );		/* Log the error		  */

    drvSerReleaseAndFree( dp, (char *)gpsmsgp );
    gps_pwroff( dp );			/* Turn off GPS power		  */
    dp->drv_flags &= ~(GPS_IN_USE | GPS_TRACKING | DO_INIT);
					/* Show we're done using GPS	  */
} /* gps10_drv() */


/****************************************************************************/
/* Function    : get_fix						    */
/* Purpose     : Get a position fix from GPS receiver			    */
/* Inputs      : Driver pointer, Sample pointer, Buffer pointer		    */
/* Outputs     : OK or ERROR						    */
/****************************************************************************/
	Errno
get_fix( Driver *dp, GpsSample *samplep, GpsMsg *gpsmsgp )
{
    Reg Nat16	i;
    Errno	rtn;

    for ( i = 0; i < 8; i++ )		/* Try 8 times to get a fix	*/
	if ( (rtn = getAndCheckGpsMsg(dp, gpsmsgp)) == POSITION_MSG )
	{
	    samplep->gps_lat = swaplong(gpsmsgp->positionMsg.posLat);
	    samplep->gps_long = swaplong(gpsmsgp->positionMsg.posLong);
	    return( OK );
	}

    samplep->gps_lat = LATLONG_ERR;	/* Return error in position	*/
    samplep->gps_long = LATLONG_ERR;

    return( rtn );

} /* get_fix() */


/************************************************************************/
/* Function    : get_status						*/
/* Purpose     : Get GPS status message, return status word		*/
/* Inputs      : Driver pointer, Buffer pointer				*/
/* Outputs     : Status Word, ERROR if failed				*/
/************************************************************************/
	Word
get_status( Driver *dp, GpsMsg *gpsmsgp )
{
    Nat16	i;

    for ( i = 0; i < 10; i++ )		/* Try 10 times to get status	*/
	if ( getAndCheckGpsMsg(dp, gpsmsgp) == GENERAL_STATUS_MSG )
	{
	    gpsAlmanacAge = swap(gpsmsgp->generalStatus.almanacAge);
	    return( ((Word)(gpsmsgp->generalStatus.rcvrMode) << 8) |
		    (gpsmsgp->generalStatus.rcvrFlags) );
	}

    return( ERROR );

} /* get_status() */


/************************************************************************/
/* Function    : get_almanac						*/
/* Purpose     : Leave GPS on long enough to get a current almanac	*/
/* Inputs      : Driver pointer, Buffer pointer				*/
/* Outputs     : None							*/
/************************************************************************/
	Void
get_almanac( Driver *dp, GpsMsg *gpsmsgp )
{
    TimeOfDay	start;

    start = tod;

    while ( (Nat16)(tod - start) < GPS_ALMANAC_TIME )
    {
	get_status( dp, gpsmsgp );
	if ( gpsAlmanacAge <= GPS_ALMANAC_AGE )
	    return;
    }
    
} /* get_almanac() */


/************************************************************************/
/* Function    : compute_mean						*/
/* Purpose     : Compute the mean position from GPS samples		*/
/* Inputs      : Sample ptr, gps record ptr				*/
/* Outputs     : Number of valid GPS samples				*/
/* Comment     : To avoid overflow, accumulates differences from first	*/
/*		 sample, and when done adds first sample back into result*/
/************************************************************************/
	Nat16
compute_mean( Reg GpsSample *samplep, Reg GpsRec *gpsrp )
{
    Reg Nat16		i, nsamples;
    Reg GpsSample	*lp;
    Reg Int32		lat, lon, baseLat, baseLong;

    lat = 0L;				/* Clear lat & long sums	*/
    lon = 0L;

    for ( i = nsamples = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ )
	if ( lp->gps_lat != LATLONG_ERR )
	{				/* Accumulate only for valid samples*/
	    if ( nsamples == 0 )
	    {
		baseLat = lp->gps_lat;
		baseLong = lp->gps_long;
	    }
	    
	    lat += (lp->gps_lat - baseLat);
	    lon += (lp->gps_long - baseLong);
	    nsamples++;
	}

    if ( nsamples > 0 )
    {					/* Now calculate means		*/
	gpsrp->gps_lat  = (lat / nsamples) + baseLat;
	gpsrp->gps_long = (lon / nsamples) + baseLong;
    }

    asm push nsamples;			/* Save number of samples	*/
    dispatch();				/* Mean calc takes a while	*/
    asm pop nsamples;			/* Restore nsamples		*/

    return( nsamples );			/* Return number of good samples*/

} /* compute_mean() */


#if ( GPS_TYPE >= 10 )		/* "Full-feature" driver calculates	*/
				/*  standard deviation, discards outlyers*/
/************************************************************************/
/* Function    : compute_err						*/
/* Purpose     : Compute sum of squares of errors of positions, in meters^2*/
/* Inputs      : Sample ptr, gps record ptr				*/
/* Outputs     : None							*/
/* Comment     : Computes variance in meters^2, puts answer in gpsrp	*/
/*		 Takes approx 14.7 ms at 9.8 MHz			*/
/************************************************************************/
	Void
compute_err( GpsSample *samplep, Reg GpsRec *gpsrp )
{
    Reg GpsSample	*lp;
    Reg Int16		i, nsamples;
    Reg Nat32		newerr;
    
    gpsrp->gps_pos_err = 0L;

    for ( i = nsamples = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ )
	if ( lp->gps_lat < LATLONG_ERR )
	{
	    newerr = compute_one_err10( lp, gpsrp );
	    if ( newerr != ULONG_MAX )
	    {
		gpsrp->gps_pos_err += newerr;
		nsamples++;
	    }
	}

    if ( nsamples > 0 )
	gpsrp->gps_pos_err /= nsamples;

    dispatch();				/* Error calcs take a while	*/

} /* compute_err() */


/************************************************************************/
/* Function    : compute_values						*/
/* Purpose     : Compute mean, sum of errors, and number samples for GPS*/
/* Inputs      : Driver ptr, Sample ptr, gps record ptr			*/
/* Outputs     : None							*/
/************************************************************************/
	Void
compute_values( Driver *dp, GpsSample *samplep, GpsRec *gpsrp )
{
    Reg Int16		i;
    Reg GpsSample	*lp;
    Nat16		nsamples;
    Reg Nat32		errorThreshhold;

    compute_mean( samplep , gpsrp );	/* Compute mean of all records	   */
    compute_err( samplep, gpsrp );	/* Compute variance of all recs*/
    errorThreshhold = max(gpsrp->gps_pos_err, GPS_ERR_THRESH);

    for ( i = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ )
    {					/* Loop to discard outlyer samples*/
	if ( lp->gps_lat < LATLONG_ERR )
	    if ( (compute_one_err10(lp, gpsrp)/4) > errorThreshhold )
		lp->gps_lat = LATLONG_ERR;
    }

    dispatch();				/* Above loop takes 14.7 ms	   */
    nsamples = compute_mean( samplep, gpsrp );
					/* Compute mean of remaining samples*/
    compute_err( samplep, gpsrp );	/* Compute variance of rem samples  */
    gpsrp->gps_samples = nsamples;	/* Store number of good samples	    */

} /* compute_values() */


#else /* GPS_TYPE < 10, reduced driver */

/************************************************************************/
/* Function    : compute_values						*/
/* Purpose     : Compute mean, sum of errors, and number samples for GPS*/
/* Inputs      : Driver ptr, Sample ptr, gps record ptr			*/
/* Outputs     : None							*/
/************************************************************************/
	Void
compute_values( Driver *dp, GpsSample *samplep, GpsRec *gpsrp )
{
    gpsrp->gps_samples = compute_mean( samplep, gpsrp );
    gpsrp->gps_pos_err = 0L;

} /* compute_values() */

#endif /* GPS_TYPE < 10, reduced driver */
@