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


4.4
date     2001.06.19.12.13.36;  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-1999 MBARI                                                 */
/****************************************************************************/
/* $Header: garmin.c,v 1.1 2001/06/19 11:43:29 oasisa Exp $			    */
/* Summary  : Driver Routines for Garmin GPS as installed on OASIS Mooring  */
/* Filename : garmin.c							    */
/* Author   : Robert Herlien (rah)					    */
/* Project  : OASIS Mooring						    */
/* $Revision: 1.1 $							    */
/* Created  : 3/31/99 from gps10.c					    */
/*									    */
/* 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:						    */
/* 31mar99 rah - created from gps10.c					    */
/* $Log:	garmin.c,v $
 * Revision 1.1  2001/06/19  11:43:29  11:43:29  oasisa (Oasis users)
 * Initial revision
 * 
 * Revision 4.3  99/06/16  10:35:55  10:35:55  bobh (Bob Herlien)
 * Mar/May '99 Deployments of M3/M2
 * 
*/
/****************************************************************************/

#include <types.h>			/* MBARI type definitions	    */
#include <const.h>			/* MBARI constants		    */
#include <oasis.h>			/* OASIS controller definitions	    */
#include <task.h>			/* OASIS Multitasking definitions   */
#include <io.h>				/* OASIS I/O definitions	    */
#include <log.h>			/* Log record definitions	    */
#include <custom.h>			/* GPS_LAT definition		    */
#include <string.h>			/* String library functions	    */
#include <limits.h>			/* Ranges for type definitions	    */
#include <stdio.h>			/* Standard I/O functions	    */
#include "garmin.h"			/* Defs for Garmin board	    */

#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 error );
Extern Void	xputs( const char *s );
Extern Void	xprintf( const char *format, ... );
Extern Int16	xgetc_tmout( Nat16 tmout  );
Extern Int16	xgets_tmout( char *s, Int16 len, Nat16 tmout );
Extern Void	xflush_ser( Nat16 timeout );
Extern char 	*find_str( char *src, char *tgt );
Extern MBool	getnum( char **s, Int16 *result, Nat16 radix );
Extern MBool	getFixedPt( char **s, Int16 *resultPtr, Nat16 decDigits );
Extern Void	bzero( void *s, int n );
Extern Void	tmpFree( char *ptr );
#if ( GPS_TYPE == 12 )
Extern Nat32	compute_one_err( GpsSample *samplep, GpsStruct *gpsP );
#endif


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

Extern Reg TimeOfDay	tod;		/* Current time in TimeOfDay format */
Extern Reg DateTime	dttm;		/* Current date & time in DateTime  */


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

Int16	get_fix( Driver *dp, GpsStruct *gpsP );
#if (GPS_TYPE != GARMIN12_TINY)
Void	compute_values( GpsStruct *gpsP );
#endif

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

#if ( GPS_TYPE == 12 )		/* 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		gpsVelAvg;	/* GPS velocity averaging time	    */
#endif


/************************************************************************/
/* Function    : turnOffMsgs						*/
/* Purpose     : Turn off Position Messages				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
	Void
turnOffMsgs( Void )
{
    xputs("$PGRMO,,2\r\n");
    
} /* turnOffMsgs() */


/************************************************************************/
/* Function    : gps_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
garmin_wake( Driver *dp, MBool on )
{
    if ( on && (dp->drv_flags & GPS_IN_USE) )
	return( FALSE );

    turnOffMsgs();
    xflush_ser(TICKS_PER_SECOND/2);

    return( TRUE );

} /* garmin_wake() */



/************************************************************************/
/* Function    : gpsInit						*/
/* Purpose     : Initialize Magellan GPS receiver			*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
	Void
gpsInit( Void )
{
    turnOffMsgs();			/* Turn off GPS messaging	  */
#if ( GPS_TYPE == 12 )		/* Long version, with user-supplied lat/long*/
    xputs("$PGRMI,");
    if ( gpsInitLat >= 0 )
	xprintf("%d,N,", gpsInitLat);
    else
	xprintf("%d,S,", -gpsInitLat);
    if ( gpsInitLong >= 0 )
	xprintf("%d,E", gpsInitLong);
    else
	xprintf("%d,W", -gpsInitLong);
#else				/* Compiled-in initial lat/long		*/
#if ( GPS_LAT >= 0 )    
    xprintf("$PGRMI,%d,N,", GPS_LAT);
#else
    xprintf("$PGRMI,%d,S,", -GPS_LAT);
#endif
#if ( GPS_LONG >= 0 )    
    xprintf("%d,E", GPS_LONG);
#else
    xprintf("%d,W", -GPS_LONG);
#endif
#endif

    xprintf(",%02d%02d%02d,%02d%02d%02d,R\r\n", dttm.dt_day, dttm.dt_mo,
	    dttm.dt_yr, dttm.dt_hr, dttm.dt_min, dttm.dt_sec);
    xflush_ser(TICKS_PER_SECOND/2);
#if ( GPS_TYPE == 12 )		/* Long version, with user-supplied alt/avg*/
    xprintf("$PGRMC,2,%d,,,,,,,A,,%d,1\r\n", gpsInitAlt, gpsVelAvg);
#else				/* Compiled-in defaults			   */
    xprintf("$PGRMC,2,%d,,,,,,,A,,,1\r\n", GPS_ALT);
#endif
    xflush_ser(TICKS_PER_SECOND/2);
    
} /* gpsInit() */


/************************************************************************/
/* Function    : garmin_drv						*/
/* Purpose     : Garmin GPS driver					*/
/* Inputs      : Driver Pointer						*/
/* Outputs     : None							*/
/************************************************************************/
	Void
garmin_drv( Driver *dp )
{
    Int16	samples, fix,i;		/* Num samples we want, sample num*/
    Errno	gpsRtn;			/* GPS return status		  */
    GpsStruct	*gpsP;			/* Sample buffer		  */
    Reg Nat16	size;			/* Size of buffers		  */
    TimeOfDay	start_time;		/* Time we started sampling	  */

    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)) + sizeof(GpsStruct);

    if ( (gpsP = (GpsStruct *)drvSerPortAndMalloc(dp, size)) == NULL )
	return;				/* Get message and sample buffers */

    bzero( (void *)gpsP, size );
    task_delay( 2 * TICKS_PER_SECOND );	/* Wait for it to rcvr to init	  */
					/* Init samples to error	  */
    if ( dp->drv_flags & DO_INIT )	/* If got userif message to init  */
	gpsInit();			/*  initialize GPS receiver	  */

    dp->drv_flags = GPS_IN_USE;		/* Show we're using GPS		  */

    xputs("$PGRMO,GPGGA,1\r\n");	    /* Turn on GPS messaging	  */
    xputs("$PGRMO,PGRMT,1\r\n");
    xflush_ser(TICKS_PER_SECOND/2);

    start_time = tod;
    while ( (tod - start_time) < dp->drv_parms[PARM1] )
    {					/* Get first fix from GPS rcvr	  */
	if ( (gpsRtn = get_fix(dp, gpsP)) != NOT_AVAIL_ERR )
	    break;			/* If got fix or timed out, stop  */
    }
    
    if ( gpsRtn == OK )			/* Got first fix		  */
    {

#if (GPS_TYPE == GARMIN12_TINY)
	for ( fix = 1; fix < samples; fix++ )
	{
	    if ( get_fix(dp, gpsP) < 0 )
		break;			 /* Get the samples, quit if time out*/
	    task_delay( TICKS_PER_SECOND/4 );	/* Let other tasks run	  */
	}
        for ( i = gpsP->gps_totSamples-1;i > 0; i--){
	  if (gpsP->gps_sample[i].gps_lat != LATLONG_ERR ){
	    gpsP->gps_logRec.gps_lat  = gpsP->gps_sample[i].gps_lat;
	    gpsP->gps_logRec.gps_long = gpsP->gps_sample[i].gps_long;
	    break;
	  }else{
	    gpsP->gps_logRec.gps_lat  = LATLONG_ERR;
	    gpsP->gps_logRec.gps_long = LATLONG_ERR;
	  }
	}
 	gpsP->gps_logRec.gps_pos_err = 0L;
#else
	for ( fix = 1; fix < samples; fix++ )
	{
	    if ( get_fix(dp, gpsP) < 0 )
		break;			 /* Get the samples, quit if time out*/
	    task_delay( TICKS_PER_SECOND/4 );	/* Let other tasks run	  */
	}

	compute_values(gpsP);		/* Compute mean, errors		  */
#endif
	gpsP->gps_logRec.gps_ontime = tod - start_time;
	gpsP->gps_logRec.gps_fmt = GARMIN_FMT;
	drvLog( dp, (Byte *)&gpsP->gps_logRec, sizeof(GpsLogRec) );
    }
    else
	drvLogError( dp, gpsRtn );	/* Log the error		  */
    /*turnOffMsgs();*//*for debug*/
    drvSerReleaseAndFree( dp, (char *)gpsP ); /* Release ser port & buffers*/
    drv_pwroff( dp );			/* Turn off GPS power		  */
    dp->drv_flags &= ~(GPS_IN_USE | GPS_TRACKING | DO_INIT );
					/* Show we're done using GPS	  */
} /* garmin_drv() */


/************************************************************************/
/* Function    : skip_field						*/
/* Purpose     : skip over n commas of GPS message			*/
/* Inputs      : String pointer, number of fields to skip		*/
/* Outputs     : Pointer to first char after nth ','			*/
/************************************************************************/
	char *
skip_field( Reg char *p, Nat16 num )
{
    for ( ; num && *p; p++ )
    {
	if ( *p == ',' )
	    num--;
    }

    return( p );

} /* skip_field() */


/************************************************************************/
/* Function    : get_status						*/
/* Purpose     : Get GPS status message, return status word		*/
/* Inputs      : Buffer pointer						*/
/* Outputs     : Status Word, ERROR if failed				*/
/************************************************************************/
	Word
get_status( char *buffer )
{
    Reg char	*p;
    Reg Int16	i;
    Reg Word	sts;
    Int16	temp;

    if ( (p = find_str(buffer, "$PGRMT")) == NULL )
	return( ERROR );		/* Look for status message ID	*/

    p = skip_field( p, 1 );		/* Go to next field		*/
    for ( i = sts = 0; i < 5; i++ )	/* Loop for status fields	*/
    {
	p = skip_field( p, 1 );		/* Go to next field		*/
	if ((*p == 'P') || (*p == 'R'))
	    sts |= 1 << i;
    }

    p = skip_field( p, 2 );		/* Go to next field		*/
    getnum(&p, &temp, 10);		/* Get board temperature	*/
    p = skip_field( p, 1 );
    if ( *p == 'R' )
	sts |= 0x20;
    
    return( ((temp & 0xff) << 8) | (sts & 0xff) );
					/* Return constructed status msg*/
} /* get_status() */


/************************************************************************/
/* Function    : get_latlong						*/
/* Purpose     : Get a latitude or longitude from GPS message		*/
/* Inputs      : Pointer to message, place to put result		*/
/* Outputs     : TRUE if got valid latlong position, else FALSE		*/
/* Comment     : Stores result as .0001 minutes N or E			*/
/************************************************************************/
	MBool
get_latlong( char **p, Int32 *result )
{
    Nat16	degmin, frac;		/* Degree/minute part, fraction part*/

    if ( !getnum(p, (Int16 *)&degmin, 10) )	/* Get degrees+minutes   */
	return( FALSE );
    if ( !getFixedPt(p, (Int16 *)&frac, 4) )	/* Get fractional minutes*/
	return( FALSE );

    degmin = ((degmin / 100) * 60) + (degmin % 100); /* Convert to minutes*/

    *p = skip_field( *p, 1 );

    switch ( **p )
    {
      case 'N':
      case 'E':
	*result = ((Int32)degmin * 10000) + frac;
	break;

      case 'S':
      case 'W':
	*result = -(((Int32)degmin * 10000) + frac);
	break;

      default:
	return( FALSE );
    }

    *p = skip_field( *p, 1 );

    return( TRUE );

} /* get_latlong() */


/****************************************************************************/
/* Function    : get_one_fix						    */
/* Purpose     : Look at one GPS output message to find a position msg	    */
/* Inputs      : Buffer pointer, Pointer to result, Timeout in seconds	    */
/* Outputs     : OK (0) if got fix, SYNC_ERR (-7) if not position msg	    */
/*		 NO_DATA if timed out, NOT_AVAIL_ERR (-2) if GPS not available*/
/****************************************************************************/
	Int16
get_one_fix( GpsStruct *gpsP, Nat16 tmout, GpsSample *sampleP )
{
    Reg Int16		rtn;
    Int16		i;
    char		*p;
    char		*buffer;

    buffer = gpsP->gps_buffer;

    while ( (rtn = xgets_tmout( buffer, GPS_BUFSIZE, tmout )) <= 0 )
	if ( rtn == ERROR )			/* If CR or LF, retry	*/
	    return( NO_DATA );

    if ( (p = find_str(buffer, "$GPGGA")) == NULL )
    {
#if (GPS_TYPE != GARMIN12_TINY)
	if ( (rtn = get_status(buffer)) != ERROR )
	    gpsP->gps_logRec.gps_status = rtn;
#endif
	return( SYNC_ERR );
    }

    p = skip_field( p, 2 );

    if ( !get_latlong(&p, &sampleP->gps_lat) )
	return( SYNC_ERR );

    if ( !get_latlong(&p, &sampleP->gps_long) )
	return( SYNC_ERR );

    if ( !getnum(&p, &i, 10) )
	return( SYNC_ERR );

    if ( i == 2 )
	gpsP->gps_logRec.gps_diffSamples++;

    p = skip_field( p, 2 );
    getFixedPt(&p, (Int16 *)&gpsP->gps_logRec.gps_hdop, 1);

    return( i == 0 ? NOT_AVAIL_ERR : OK );

} /* get_one_fix() */


/****************************************************************************/
/* Function    : get_fix						    */
/* Purpose     : Get a position fix from GPS receiver			    */
/* Inputs      : Driver pointer, GPS data struct			    */
/* Outputs     : OK (0) if got fix, SYNC_ERR (-7) if not position msg	    */
/*		 ERROR if timed out, NOT_AVAIL_ERR (-2) if GPS not available*/
/****************************************************************************/
	Int16
get_fix( Driver *dp, GpsStruct *gpsP )
{
    Nat16		i;
    Int16		rtn;
    Reg GpsSample	*sampleP;

    sampleP = &gpsP->gps_sample[gpsP->gps_totSamples];

    for ( i = 0; i < 10; i++ )		/* Try 10 times to get a fix	*/
    {
	if ( (rtn = get_one_fix(gpsP, dp->drv_parms[TIMEOUT], sampleP))
	     != SYNC_ERR )
	    break;
    }
    
    if ( rtn == OK )			/* If good fix, incr sample num */
	gpsP->gps_totSamples++;
    else
    {					/* If didn't get good position,	*/
	sampleP->gps_lat = LATLONG_ERR;	/*  return error in position	*/
	sampleP->gps_long = LATLONG_ERR;
    }

    task_delay( TICKS_PER_SECOND/4 );	/* Allow someone else to run	*/
    return( rtn );

} /* get_fix() */

#if (GPS_TYPE != GARMIN12_TINY)

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

    lat = 0L;				/* Clear lat & long sums	*/
    lon = 0L;
    lp = gpsP->gps_sample;

    for ( i = nsamples = 0; i < gpsP->gps_totSamples; 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		*/
	gpsP->gps_logRec.gps_lat  = (lat / nsamples) + baseLat;
	gpsP->gps_logRec.gps_long = (lon / nsamples) + baseLong;
    }

    gpsP->gps_logRec.gps_avgSamples = nsamples;
    dispatch();				/* Mean calc takes a while	*/

} /* compute_mean() */
#endif

#if ( GPS_TYPE == 12 )		/* "Full-feature" driver calculates	*/
				/*  standard deviation, discards outlyers*/
/************************************************************************/
/* Function    : compute_err						*/
/* Purpose     : Compute sum of squares of errors of positions, in meters^2*/
/* Inputs      : GPS data struct 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( GpsStruct *gpsP )
{
    Reg GpsSample	*lp;
    Reg Int16		i, nsamples;
    Reg Nat32		newerr;
    
    gpsP->gps_logRec.gps_pos_err = 0L;
    lp = gpsP->gps_sample;
    
    for ( i = nsamples = 0; i < gpsP->gps_totSamples; i++, lp++ )
	if ( lp->gps_lat < LATLONG_ERR )
	{
	    newerr = compute_one_err( lp, gpsP );
	    if ( newerr != ULONG_MAX )
	    {
		gpsP->gps_logRec.gps_pos_err += newerr;
		nsamples++;
	    }
	}

    if ( nsamples > 0 )
	gpsP->gps_logRec.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      : GPS data struct ptr					*/
/* Outputs     : None							*/
/************************************************************************/
	Void
compute_values( GpsStruct *gpsP )
{
    Reg Int16		i;
    Reg GpsSample	*lp;
    Nat16		nsamples;
    Reg Nat32		errorThreshhold;

    compute_mean( gpsP );		/* Compute mean of all records	   */
    compute_err( gpsP );		/* Compute variance of all recs*/
    errorThreshhold = max(gpsP->gps_logRec.gps_pos_err, GPS_ERR_THRESH);
    lp = gpsP->gps_sample;

    for ( i = 0; i < gpsP->gps_totSamples; i++, lp++ )
    {					/* Loop to discard outlyer samples*/
	if ( lp->gps_lat < LATLONG_ERR )
	    if ( (compute_one_err(lp, gpsP)/4) > errorThreshhold )
		lp->gps_lat = LATLONG_ERR;
    }

    dispatch();				/* Above loop takes 14.7 ms	    */
    compute_mean( gpsP );		/* Compute mean of remaining samples*/
    compute_err( gpsP );		/* Compute variance of rem samples  */

} /* compute_values() */


#else /* GPS_TYPE != 12, reduced driver */
#if (GPS_TYPE != GARMIN12_TINY)
/************************************************************************/
/* Function    : compute_values						*/
/* Purpose     : Compute mean, sum of errors, and number samples for GPS*/
/* Inputs      : GPS data struct ptr					*/
/* Outputs     : None							*/
/************************************************************************/
	Void
compute_values( GpsStruct *gpsP )
{
    compute_mean( gpsP );
    gpsP->gps_logRec.gps_pos_err = 0L;

} /* compute_values() */
#endif
#endif /* GPS_TYPE != 12, reduced driver */
@