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


4.4
date     2001.06.19.12.14.01;  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: gpss.s,v 1.1 2001/06/19 11:43:45 oasisa Exp $			    *
;* Summary  : Assembly language routines for Magellan GPS module	    *
;* Filename : gpss.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_err

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


; Following equates are to convert errors in .001 min^2 to meters^2
; The conversion factor for latitude is 3.43, which is approx 55/16
; The conversion factor for longitude contains a factor of cos(lat)^2, 
; and hence is only valide at 36d45' latitude.  Nevertheless, it's not too
; far off at other latitudes.  The longitude factor is 2.20, or approx 35/16

LATMUL		equ	55		;Numerator of latitude conversion factor
LONMUL		equ	35		;Numerator of longitude conversion
LATLONSHFT	equ	4		;Denominator, as a shift factor


		RSEG

laterr:		dsl	1		;Latitude error


		CSEG

;******************************************************************************
; compute_one_err - Compute error in meters^2 for one GPS sample
;
; Nat32 compute_one_err(LatLong *samplep, GPSRec *gpsrp)
;
; Computes the following result, with appropriate checks for overflow:
;
;  laterr = samplep->po_lat - gpsrp->gps_lat;
;  longerr = samplep->po_long - gpsrp->gps_long;
;  return((LATMUL * laterr * laterr) + (LONMUL * longerr * longerr) / LLDIV);
;
; Done in assembly language because there's no convenient way in C to do
; the appropriate overflow checks
;
compute_one_err:
	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]
	bge	err1			;Check if negative
	neg	laterr			;If negative, take absolute value
	inc	laterr + 2		;Check for MSW == ffff
err1:
	cmp	laterr+2, R0		;If MSW != 0, error is too big for
	bne	err_out_of_range	;  following calculations

	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
	bge	err2			;Again check if negative
	neg	tmp0			;If negative, take absolute value
	inc	tmp2			;Check for MSW == ffff
err2:
	cmp	tmp2, R0		;If MSW != 0, error is too big for
	bne	err_out_of_range	;  following calculations

	mulu	laterr, laterr		;Square the latitude error
	mulu	tmp4, laterr+2, #LATMUL	;Multiply MSW of laterr^2 by mult fact
	cmp	R0, tmp6		;If MSW of result != 0, overflow
	bne	err_out_of_range
	mulu	laterr, #LATMUL		;Multiply LSW of laterr^2 by mult fact
	add	laterr+2, tmp4		;Add LSW of MSW mult to MSW of result
	bc	err_out_of_range	;If carry, we're out of range

	mulu	tmp0, tmp0		;Square the longitude error
	mulu	tmp4, tmp2, #LONMUL	;Multiply MSW of longerr^2 by mult fact
	cmp	R0, tmp6		;If MSW of result != 0, overflow
	bne	err_out_of_range
	mulu	tmp0, #LONMUL		;Multiply LSW of laterr^2 by mult fact
	add	tmp2, tmp4		;Add LSW of MSW mult to MSW of result
	bc	err_out_of_range	;If carry, we're out of range

	add	tmp0, laterr		;Add multiplier*laterr^2 to lon result
	addc	tmp2, laterr + 2
	bc	err_out_of_range	;If carry, we're out of range
	shrl	tmp0, #LATLONSHFT	;Divide by shift factor
	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
@