head 4.4; access ; symbols ; locks oasisa:4.4; strict; comment @ * @; 4.4 date 2001.06.19.12.13.51; 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 MBARI */ /****************************************************************************/ /* $Header: gps.c,v 1.1 2001/06/19 11:43:38 oasisa Exp $ */ /* Summary : Driver Routines for Magellan GPS as installed on OASIS Mooring*/ /* Filename : gps.c */ /* Author : Robert Herlien (rah) */ /* Project : OASIS Mooring */ /* $Revision: 1.1 $ */ /* Created : 11/22/91 */ /* */ /* 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: */ /* 22nov91 rah - created */ /* $Log: gps.c,v $ * Revision 1.1 2001/06/19 11:43:38 11:43:38 oasisa (Oasis users) * Initial revision * * Revision 4.1 98/05/12 09:35:11 09:35:11 bobh (Bob Herlien) * June '98 turnaround for EqPac * * Revision 3.4 96/06/18 15:24:27 15:24:27 bobh (Bob Herlien) * June '96 deployment of M1 * * Revision 3.3 95/04/13 13:46:59 13:46:59 hebo (Bob Herlien) * Drifter Deployment for Coop (flip) cruise * * Revision 3.1 95/03/09 19:31:03 19:31:03 hebo (Bob Herlien) * March '95 Deployment of M1A * * Revision 3.0 95/02/21 18:42:48 18:42:48 hebo (Bob Herlien) * February '95 Deployment * * Revision 2.4 93/10/29 13:57:01 13:57:01 hebo (Bob Herlien) * November 1993 Deployment * * Revision 2.3 93/10/12 08:29:44 08:29:44 hebo (Bob Herlien) * Oct '93 Deployment of M2 * * Revision 2.2 93/04/06 11:54:54 11:54:54 hebo (Bob Herlien) * Minor change to GPS for 5-channel Magellan * * Revision 2.0 92/08/21 14:45:08 14:45:08 hebo (Bob Herlien 408-647-3748) * August 1992 deployment * * Revision 1.3 92/03/05 17:12:37 17:12:37 hebo (Bob Herlien 408-647-3748) * New defaults, restart check, perm power stuff, analog command * */ /****************************************************************************/ #include /* MBARI type definitions */ #include /* MBARI constants */ #include /* OASIS controller definitions */ #include /* OASIS Multitasking definitions */ #include /* OASIS I/O definitions */ #include /* Log record definitions */ #include /* String library functions */ #include /* Ranges for type definitions */ #include /* Standard I/O functions */ #pragma(noreentrant) /* This module non-reentrant */ #define GPS_SAMPLES 200 /* Max number GPS samples to take */ #define GPS_BUFSIZE 128 /* Buffer size for GPS messages */ #define GPS_ALM_TIME 48 /* Num samples between trying almanac*/ #define GPS_CLK_UPDATE 3 /* Update clock if off > this # secs*/ #define LATLONG_ERR LONG_MAX typedef struct /************************************/ { /* One Lat-Long position */ Int32 po_lat; /* Units of .001 minutes N latitude */ Int32 po_long; /* Units of .001 minutes E longitude*/ } LatLong; /************************************/ typedef struct /************************************/ { /* GPSRec - GPS Position record */ Int32 gps_lat; /* Units of .001 minutes North */ Int32 gps_long; /* Units of .001 minutes West */ Nat32 gps_err; /* Variance (meters ^2) */ Nat16 gps_samples; /* Number of samples */ Word gps_status; /* Status word */ } GPSRec; /************************************/ /* Status word fields: */ #define STS_GQ 0x000f /* Geometric quality field */ #define STS_SQ 0x00f0 /* Signal quality field */ #define STS_STATE 0x0700 /* State: 0 = INI, 1 = IDL, 2 = STS */ /* 3 = ALM, 4 = EPH, 5 = ACQ, */ /* 6 = POS, 7 = NAV */ #define STS_BATT 0x0800 /* Backup battery power low */ #define STS_ALM 0x3000 /* Almanac needs updating */ #define STS_OSC 0x4000 /* Oscillator out of tune */ #define STS_MEM 0x8000 /* Lost memory */ /************************************/ /* RETURN CODES in got_gps */ #define GPS_FIX 1 /* Return code when got GPS position*/ #define GPS_NOTPSN 0 /* GPS message wasn't a position msg*/ #define GPS_TMOUT ERROR /* Timed out getting GPS message */ #define GPS_NOTAVAIL -2 /* Returned GPS not available msg */ /************************************/ #define IN_USE_BIT 0x80 /* Use MSB of drv_flags for gps in use*/ /********************************/ /* External Functions */ /********************************/ Extern Void drv_ser_port( Driver *dp ); Extern Void drv_ser_release( Driver *dp ); 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 ); 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 char *find_str( char *src, char *tgt ); Extern MBool getnum( char **s, Int16 *result, Nat16 radix ); Extern Int16 getByte( char *p, Nat16 radix ); Extern Void bzero( void *s, int n ); Extern char *tmpMalloc( Nat16 size ); Extern Void tmpFree( char *ptr ); Extern TimeOfDay dtToTod( Reg DateTime *dtp ); Extern MBool clk_read( Void ); Extern Void clk_write( Reg DateTime *dtp ); Extern Void drvr_timechange( Int32 diff ); /********************************/ /* External Function */ /********************************/ Extern Nat32 compute_one_err( LatLong *samplep, GPSRec *gpsrp ); /********************************/ /* External Data */ /********************************/ Extern Reg TimeOfDay tod; /* Current time in TimeOfDay format */ Extern MBool doGpsTime; /* TRUE to update time from GPS */ /********************************/ /* Forward Declarations */ /********************************/ Void gps_msg( char *msg ); Int16 get_fix( Driver *dp, LatLong *sample, char *buffer ); Void compute_values( Driver *dp, LatLong *samplep, GPSRec *gpsrp ); Void turnOffMsgs( Driver *dp ); Word get_status( Driver *dp, char *buffer ); Void chk_alm( Driver *dp, char *buffer ); Void getGPStime( Driver *dp, char *buffer ); /********************************/ /* Global Data */ /********************************/ Global MBool doGpsTime; /* TRUE to update time from GPS */ /********************************/ /* Module Local Data */ /********************************/ MLocal const struct { Word sts_mask; Nat16 sts_shft; } sts_bits[] = { {1, 11}, {1, 14}, {0x0f, 4}, {0x0f, 0}, {0, 0}, {3, 12}, {1, 15}, {7, 8} }; /************************************************************************/ /* 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 gps_wake( Driver *dp, MBool on ) { if ( on && (dp->drv_flags & IN_USE_BIT) ) return( FALSE ); gps_msg("H00,0"); /* Turn off position messages, if on */ gps_msg("B00,0"); /* Turn off status messages, if on */ if ( on ) gps_msg("100,1"); /* If connecting, turn on GPS messaging */ else /* (connect func already turned on pwr)*/ { task_delay( TICKS_PER_SECOND/2 ); drv_pwroff( dp ); /* If disconnect, turn off GPS power */ } return( TRUE ); } /* gps_wake() */ /************************************************************************/ /* Function : gps_drv */ /* Purpose : GPS driver */ /* Inputs : Driver Pointer */ /* Outputs : None */ /************************************************************************/ Void gps_drv( Driver *dp ) { Int16 samples; /* Number of samples we want */ Int16 fix; /* Position (fix) number */ Int16 got_gps; /* GPS msg status. See codes above*/ LatLong *samplep; /* Sample buffer */ char *gpsbuf; /* Buffer for messages */ Reg Nat16 size; /* Size of buffers */ 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(LatLong); if ( (gpsbuf = tmpMalloc(size + GPS_BUFSIZE)) == NULL ) return; /* Get message and sample buffers */ samplep = (LatLong *)(gpsbuf + GPS_BUFSIZE); bzero( (void *)samplep, size ); drv_ser_port( dp ); /* Get serial port access */ task_delay( 2 * TICKS_PER_SECOND ); /* Wait for it to come up */ dp->drv_flags |= IN_USE_BIT; /* Show we're using GPS */ /* Init samples to error */ if ( dp->drv_flags & DO_INIT ) { /* If got userif message to init */ xputs("$PMGLN,01,1\n"); /* Tell it "I'm lost" */ xputs("$PMGLD,00,2\n"); /* put it in 2D mode, give postn */ xprintf("$GPGGA,0,3645,N,12202,W,0,3,0,%d,M,0,M\n", dp->drv_usrparm); } gps_msg("100,1"); /* Turn on GPS messaging */ gps_msg("B00,2"); /* Get position every second */ drv_ser_release( dp ); /* Release serial port */ for ( fix = dp->drv_parms[PARM1]; fix > 0; fix-- ) /* Get first fix from GPS rcvr */ if ( (got_gps = get_fix(dp, samplep, gpsbuf)) != GPS_NOTAVAIL ) break; /* If got fix or timed out, stop */ if ( got_gps == GPS_FIX ) /* Got first fix */ { for ( fix = 1; fix < samples; fix++ ) if ( get_fix(dp, samplep + fix, gpsbuf) < 0 ) break; /* Get the samples, quit if time out*/ gpsRec.gps_samples = fix; compute_values(dp, samplep, &gpsRec); /* Compute mean, errors */ dp->drv_cnt++; /* Almanac OK, count # rcds since alm*/ turnOffMsgs( dp ); /* Turn off position messages */ gpsRec.gps_status = get_status( dp, gpsbuf ); drvLog( dp, (Byte *)&gpsRec, sizeof(gpsRec) ); if ( doGpsTime ) getGPStime( dp, gpsbuf ); } else { turnOffMsgs( dp ); /* Turn off position messages */ drvLogError( dp ); /* Log the error */ } chk_alm( dp, gpsbuf ); /* See if need almanac update */ tmpFree( gpsbuf ); /* Release buffers */ drv_pwroff( dp ); /* Turn off GPS power */ dp->drv_flags &= ~IN_USE_BIT; /* Show we're done using GPS */ } /* gps_drv() */ /************************************************************************/ /* Function : gps_msg */ /* Purpose : Turn on/off a given message type from Magellan Rcvr */ /* Inputs : Fields 2 & 3 for $PMGLI message */ /* Outputs : None */ /************************************************************************/ Void gps_msg( char *msg ) { xprintf("$PMGLI,00,%s,A\n", msg); } /* gps_msg() */ /************************************************************************/ /* 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_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 milliminutes (.001 min) N or E */ /************************************************************************/ MBool get_latlong( char **p, Int32 *result ) { Nat16 degmin, frac; /* Degree/minute part, fraction part*/ if ( !getnum(p, (Int16 *)°min, 10) ) /* Get degrees+minutes */ return( FALSE ); if ( *(*p)++ != '.' ) /* Look for decimal point*/ return( FALSE ); if ( !getnum(p, (Int16 *)&frac, 10) ) /* Get fractional minutes*/ return( FALSE ); degmin = ((degmin / 100) * 60) + (degmin % 100); /* Convert to minutes*/ while ( frac >= 100 ) /* Fractional part must be*/ frac /= 10; /* .00 to .99 */ *result = ((Int32)degmin * 1000) + (frac * 10); *p = skip_field( *p, 1 ); switch ( **p ) { case 'N': case 'E': break; case 'S': case 'W': *result = -*result; 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 : GPS_FIX (1) if got fix, GPS_NOTPSN (0) if not position msg */ /* ERROR if timed out, GPS_NOTAVAIL (-2) if GPS not available */ /****************************************************************************/ Int16 get_one_fix( char *buffer, LatLong *samplep, Nat16 tmout ) { Int16 i; char *p; while ( (i = xgets_tmout( buffer, GPS_BUFSIZE, tmout )) == 0 ) ; /* If CR or LF, retry */ if ( i == ERROR ) return( ERROR ); if ( (p = find_str(buffer, "$GPGGA")) == NULL ) return( GPS_NOTPSN ); p = skip_field( p, 2 ); if ( !get_latlong(&p, &samplep->po_lat) ) return( GPS_NOTPSN ); if ( !get_latlong(&p, &samplep->po_long) ) return( GPS_NOTPSN ); if ( !getnum(&p, &i, 10) ) return( GPS_NOTPSN ); return( i == 1 ? GPS_FIX : GPS_NOTAVAIL ); } /* get_one_fix() */ /****************************************************************************/ /* Function : get_fix */ /* Purpose : Get a position fix from GPS receiver */ /* Inputs : Driver pointer, Sample pointer, Buffer pointer */ /* Outputs : GPS_FIX (1) if got fix, GPS_NOTPSN (0) if no position msgs */ /* ERROR if timed out, GPS_NOTAVAIL (-2) if GPS not available */ /****************************************************************************/ Int16 get_fix( Driver *dp, LatLong *samplep, char *buffer ) { Nat16 i; Int16 rtn; drv_ser_port( dp ); /* Get serial port access */ for ( i = 0; i < 6; i++ ) /* Try 6 times to get a fix */ if ( (rtn = get_one_fix(buffer, samplep, dp->drv_parms[TIMEOUT])) != GPS_NOTPSN ) break; if ( rtn != GPS_FIX ) { /* If didn't get good position, */ samplep->po_lat = LATLONG_ERR; /* return error in position */ samplep->po_long = LATLONG_ERR; } drv_ser_release( dp ); /* Release serial port */ task_delay( TICKS_PER_SECOND/2 ); /* Allow someone else to run */ return( rtn ); } /* get_fix() */ /************************************************************************/ /* Function : turnOffMsgs */ /* Purpose : Turn off Position Messages */ /* Inputs : Driver pointer */ /* Outputs : None */ /************************************************************************/ Void turnOffMsgs( Driver *dp ) { drv_ser_port( dp ); /* Get serial port */ gps_msg("B00,0"); /* Turn off position messages */ task_delay( TICKS_PER_SECOND ); /* Let last message arrive, if any*/ while ( xgetc_tmout(1) != ERROR ) /* Drain serial buffer */ ; drv_ser_release( dp ); /* Release serial port */ } /* turnOffMsgs() */ /************************************************************************/ /* Function : get_status */ /* Purpose : Get GPS status message, return status word */ /* Inputs : Driver pointer, Buffer pointer */ /* Outputs : Status Word, 0 if failed */ /************************************************************************/ Word get_status( Driver *dp, char *buffer ) { Reg char *p; Reg Int16 i, skip; Reg Word sts; Int16 res; drv_ser_port( dp ); /* Get serial port */ gps_msg("H00,1"); /* Ask for receiver status */ i = xgets_tmout( buffer, GPS_BUFSIZE, dp->drv_parms[TIMEOUT] ); /* Get status message */ drv_ser_release( dp ); /* Release serial port */ if ( i <= 0 ) /* If no message, return 0 status*/ return( 0 ); if ( (p = find_str(buffer, "*")) != NULL ) *--p = '\0'; if ( (p = find_str(buffer, "$PMGLH")) == NULL ) return( 0 ); /* Look for status message ID */ sts = 0; skip = 4; for ( i = 0; i < 8; i++ ) /* Loop for all status fields */ { p = skip_field( p, skip ); /* Skip appropriate num fields */ if ( !getnum(&p, &res, 10) ) /* Get status field */ break; sts |= ((res & sts_bits[i].sts_mask) << (sts_bits[i].sts_shft)); skip = 1; /* Turn into GPSRec status */ } return( sts ); /* Return constructed status msg*/ } /* get_status() */ /************************************************************************/ /* Function : getGPStime */ /* Purpose : Get time from GPS receiver, set globally if far enough off*/ /* Inputs : Driver pointer, Buffer pointer */ /* Outputs : None */ /************************************************************************/ Void getGPStime( Driver *dp, char *buffer ) { Reg char *p; Reg Int16 i; Int16 result[3]; DateTime gpsdate; Reg Int32 timechange; drv_ser_port( dp ); /* Get serial port */ gps_msg("A00,1"); /* Ask for time and date */ i = xgets_tmout( buffer, GPS_BUFSIZE, dp->drv_parms[TIMEOUT] ); /* Get status message */ drv_ser_release( dp ); /* Release serial port */ if ( i <= 0 ) /* If no message, return 0 status*/ return; if ( (p = find_str(buffer, "$GPZDA")) == NULL ) return; /* Look for date message ID */ p = skip_field( p, 1 ); /* Skip past comma */ gpsdate.dt_hr = getByte(p, 10); /* Get hours */ gpsdate.dt_min = getByte(p+2, 10); /* Get minutes */ gpsdate.dt_sec = getByte(p+4, 10); /* Get seconds */ for ( i = 0; i < 3; i++ ) /* Loop for all date/time fields*/ { p = skip_field( p, 1 ); if ( !getnum(&p, &result[i], 10) ) /* Get date fields */ return; } gpsdate.dt_yr = result[2] % 100; gpsdate.dt_mo = result[1]; gpsdate.dt_day = result[0]; clk_read(); timechange = dtToTod(&gpsdate) - clk_read(); if ( (timechange > GPS_CLK_UPDATE) || (timechange < -GPS_CLK_UPDATE) ) { clk_write( &gpsdate ); drvr_timechange( timechange ); } } /* getGPStime() */ /************************************************************************/ /* Function : chk_alm */ /* Purpose : See if we need new almanac. Wait till it arrives if so*/ /* Inputs : Driver ptr, Buffer ptr */ /* Outputs : None */ /* Comments : Keeps GPS on until almanac status OK or times out */ /************************************************************************/ Void chk_alm( Driver *dp, char *buffer ) { Nat16 i; if ( (dp->drv_flags & DO_INIT) || (dp->drv_cnt >= GPS_ALM_TIME) ) { /* Leave power on only once/day */ dp->drv_flags &= ~DO_INIT; /* If init flag was on, turn off*/ for ( i = dp->drv_parms[PARM2]; (get_status(dp, buffer) & (STS_ALM | STS_MEM)) && (i > 0); i-- ) /* Loop checking for alm & mem OK*/ dp->drv_cnt = 0; /* Show we got new alamanac */ } } /* chk_alm() */ /************************************************************************/ /* Function : compute_mean */ /* Purpose : Compute the mean position from GPS samples */ /* Inputs : Sample ptr, gps record ptr */ /* Outputs : Number of valid GPS samples */ /* Comment : Calculates mean of samples at .001 minutes resolution */ /************************************************************************/ Nat16 compute_mean( LatLong *samplep, Reg GPSRec *gpsrp ) { Reg Nat16 i, nsamples; Reg LatLong *lp; Reg Int32 lat, lon; lat = 0L; /* Clear lat & long sums */ lon = 0L; for ( i = nsamples = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ ) if ( lp->po_lat < LATLONG_ERR ) { /* If this is a good sample */ lat += lp->po_lat; /* add into accumulated values */ lon += lp->po_long; nsamples++; } if ( nsamples > 0 ) { /* Now calculate means */ gpsrp->gps_lat = lat / nsamples; gpsrp->gps_long = lon / nsamples; } 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() */ /************************************************************************/ /* 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( LatLong *samplep, Reg GPSRec *gpsrp ) { Reg LatLong *lp; Reg Int16 i, nsamples; Reg Nat32 newerr; gpsrp->gps_err = 0L; for ( i = nsamples = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ ) if ( lp->po_lat < LATLONG_ERR ) { newerr = compute_one_err( lp, gpsrp ); if ( newerr != ULONG_MAX ) { gpsrp->gps_err += newerr; nsamples++; } } if ( nsamples > 0 ) gpsrp->gps_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, LatLong *samplep, GPSRec *gpsrp ) { Reg Int16 i; Reg LatLong *lp; Nat16 nsamples; compute_mean( samplep , gpsrp ); /* Compute mean of all records */ compute_err( samplep, gpsrp ); /* Compute variance of all recs*/ for ( i = 0, lp = samplep; i < gpsrp->gps_samples; i++, lp++ ) { /* Loop to discard outlyer samples*/ if ( lp->po_lat < LATLONG_ERR ) if ( (compute_one_err(lp, gpsrp)/4) > gpsrp->gps_err ) lp->po_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() */ @