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


4.4
date     2001.06.19.12.13.45;  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: garmins.s,v 1.1 2001/06/19 11:43:33 oasisa Exp $		    *
;* Summary  : Assembly language routines for Garmin GPS-25 module	    *
;* Filename : gpss.s							    *
;* Author   : Robert Herlien						    *
;* Project  : OASIS Mooring Controller                                      *
;* $Revision: 1.1 $							    *
;* Created  : 3/31/99 from gps10s.s					    *
;*									    *
;* 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.            *
;*									    *
;****************************************************************************

GPSS		MODULE

$INCLUDE(..\C196\INCLUDE\8096.INC)
$INCLUDE(OASIS.INC)


;*****************************************************************************

PUBLIC	compute_one_err

; Offsets into GpsStruct
;
NUM_SAMPLES	equ	0
LOGREC		equ	2		;Offset to gps_logRec
AVG_LAT		equ	LOGREC+4	;Offset to latitude, Int32
AVG_LON		equ	LOGREC+8	;Offset to longitude, Int32
ERR		equ	LOGREC+12	;Offset to gps_pos_err

; Offsets into GpsSample
LAT		equ	0		;Latitude in GpsSample
LON		equ	4		;Longitude in GpsSample


; Following equates are to convert errors in .0001 min to meters.
; The conversion factor for latitude is .1853, which is approx 16/86
; The conversion factor for longitude contains a factor of cos(lat).
; At 36d45' latitude, the conversion for longitude is .1485, which 
; is approx 16/108.  It's not too far off at lower latitudes.

LATDIV		equ	86		;Divisor of latitude conversion factor
LONDIV		equ	108		;Divisor of longitude conversion
LATSHFT		equ	4		;Multiplier, as a shift factor
LONSHFT		equ	4


		RSEG

laterr:		dsl	1		;Latitude error


		CSEG

;******************************************************************************
; compute_one_err - Compute error in meters^2 for one GPS sample
;
; Nat32 compute_one_err(GpsSample *samplep, GpsStruct *gpsP)
;
; Computes the following result, with appropriate checks for overflow:
;
;  laterr = ((samplep->gps_lat - gpsP->gps_lat)<<LATSHFT) / LATDIV;
;  longerr = ((samplep->gps_long - gpsP->gps_long)<<LONSHFT) / LONDIV;
;  return((laterr * laterr) + (longerr * longerr));
;
; Done in assembly language because there's no convenient way in C to do
; the appropriate overflow checks.  Also it's much faster.
; Overflows if either latitude or longitude error is > 32.767 km
;
compute_one_err:
	ld	tmp4, 2[SP]		;TMP4 = samplep ptr
	ld	tmp6, 4[SP]		;TMP6 = GpsStruct ptr
	ld	laterr, LAT[tmp4]	;Get latitude sample
	ld	laterr+2, LAT+2[tmp4]
	sub	laterr, AVG_LAT[tmp6]	;Subtract mean latitude from GPSRec
	subc	laterr+2, AVG_LAT+2[tmp6]
	shll	laterr, #LATSHFT
	bv	err_out_of_range	;If overflow, exit
	div	laterr, #LATDIV		;Convert to meters
	bv	err_out_of_range	;If overflow, exit
	clr	laterr + 2		;Clear MSW
	mul	laterr, laterr		;Square the latitude error

	ld	tmp0, LON[tmp4]		;Get longitude sample
	ld	tmp2, LON+2[tmp4]
	sub	tmp0, AVG_LON[tmp6]	;Subtract mean longitude from GPSRec
	subc	tmp2, AVG_LON+2[tmp6]	;Now TMP0 = longitude error
	shll	tmp0, #LONSHFT
	bv	err_out_of_range	;If overflow, exit
	div	tmp0, #LONDIV		;Convert to meters
	bv	err_out_of_range	;If overflow, exit
	clr	tmp2			;Clear MSW
	mul	tmp0, tmp0		;Square the longitude error

	add	tmp0, laterr		;Add laterr^2 to longerr^2
	addc	tmp2, laterr + 2
	bc	err_out_of_range	;If carry, we're out of range
	ret

err_out_of_range:			;Get here on any overflow condition
	ld	tmp4, 2[SP]		;Get sample ptr again
	ldbse	tmp0, #0ffh		;Return ULONG_MAX
	st	tmp0, LAT[tmp4]		;Store LONG_MAX in samplep->gps_lat
	ld	tmp2, #7fffh		;This indicates sample in error
	st	tmp2, LAT+2[tmp4]
	ret

	END
@