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


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


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



4.4
log
@New Repository; 6/19/2001 (klh)
@
text
@;****************************************************************************
;* Copyright 1995 MBARI							    *
;****************************************************************************
;* $Header: gps10s.s,v 1.1 2001/06/19 11:43:43 oasisa Exp $			    *
;* Summary  : Assembly language routines for 10 channel Magellan GPS	    *
;* Filename : gps10s.s							    *
;* Author   : Robert Herlien						    *
;* Project  : OASIS Mooring Controller                                      *
;* $Revision: 1.1 $							    *
;* Created  : 02/06/95							    *
;*									    *
;* 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_err10

; Following offsets are same for both the GpsSample struct and GpsRec struct
;
LAT		equ	0		;Offset to latitude, Int32
LON		equ	4		;Offset to longitude, Int32

LATLONDIV	equ	90	;One unit of latitude (10^-7 degrees) equals
				;1/90 meter (1.1 cm).  True also for longitude
				;only at equator, but we're approximating

		RSEG

laterr:		dsl	1		;Latitude error


		CSEG

;******************************************************************************
; compute_one_err10 - Compute error in meters^2 for one GPS sample
;
; Nat32 compute_one_err10(GpsSample *samplep, GpsRec *gpsrp)
;
; Computes the following result, with appropriate checks for overflow:
;
;  laterr = (samplep->gps_lat - gpsrp->gps_lat) / LATLONDIV;
;  longerr = (samplep->gps_long - gpsrp->gps_long) / LATLONDIV;
;  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_err10:
	ld	tmp4, 2[SP]		;TMP4 = samplep ptr
	ld	tmp6, 4[SP]		;TMP6 = GPSRec ptr
	ld	laterr, LAT[tmp4]	;Get latitude sample
	ld	laterr+2, LAT+2[tmp4]
	sub	laterr, LAT[tmp6]	;Subtract mean latitude from GPSRec
	subc	laterr+2, LAT+2[tmp6]
	div	laterr, #LATLONDIV	;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, LON[tmp6]		;Subtract mean longitude from GPSRec
	subc	tmp2, LON+2[tmp6]	;Now TMP0 = longitude error
	div	tmp0, #LATLONDIV	;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 GPSRec ptr again
	ldbse	tmp0, #0ffh		;Return ULONG_MAX
	st	tmp0, LAT[tmp4]		;Store LONG_MAX in gpsrp->gps_lat
	ld	tmp2, #7fffh		;This indicates sample in error
	st	tmp2, LAT+2[tmp4]
	ld	tmp2, tmp0		;MSW of ULONG_MAX return
	ret

	END
@