gpsfuncs.asm

OpenSource GPS is software for x86 PCs that allows you to acquire, track and demodulate signals from

	?debug	L 511
	mov	ax,seg _current_loc
	mov	es,ax
	fld	qword ptr es:_current_loc+8
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _cos
	add	sp,8
	fstp	tbyte ptr [bp-110]
	mov	ax,seg _current_loc
	mov	es,ax
	fld	qword ptr es:_current_loc
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _cos
	add	sp,8
	fld	tbyte ptr [bp-110]
	fmul	
	fstp	qword ptr [bp-230]
   ;	
   ;	   up.y=sin(current_loc.lon)*cos(current_loc.lat);
   ;	
	?debug	L 512
	mov	ax,seg _current_loc
	mov	es,ax
	fld	qword ptr es:_current_loc+8
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _sin
	add	sp,8
	fstp	tbyte ptr [bp-110]
	mov	ax,seg _current_loc
	mov	es,ax
	fld	qword ptr es:_current_loc
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _cos
	add	sp,8
	fld	tbyte ptr [bp-110]
	fmul	
	fstp	qword ptr [bp-222]
   ;	
   ;	   up.z=sin(current_loc.lat);
   ;	
	?debug	L 513
	mov	ax,seg _current_loc
	mov	es,ax
	fld	qword ptr es:_current_loc
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _sin
	add	sp,8
	fstp	qword ptr [bp-214]
   ;	
   ;	/*
   ;	     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
   ;	*/
   ;		xls =gpspos1.x-rec_pos_xyz.x;
   ;	
	?debug	L 517
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz
	fsubr	qword ptr [bp-134]
	fstp	qword ptr [bp-44]
   ;	
   ;		yls =gpspos1.y-rec_pos_xyz.y;
   ;	
	?debug	L 518
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz+8
	fsubr	qword ptr [bp-126]
	fstp	qword ptr [bp-52]
   ;	
   ;		zls =gpspos1.z-rec_pos_xyz.z;
   ;	
	?debug	L 519
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz+16
	fsubr	qword ptr [bp-118]
	fstp	qword ptr [bp-60]
   ;	
   ;		range1=sqrt(xls*xls+yls*yls+zls*zls);
   ;	
	?debug	L 520
	fld	qword ptr [bp-44]
	fmul	qword ptr [bp-44]
	fld	qword ptr [bp-52]
	fmul	qword ptr [bp-52]
	fadd	
	fld	qword ptr [bp-60]
	fmul	qword ptr [bp-60]
	fadd	
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _sqrt
	add	sp,8
	fstp	qword ptr [bp-28]
   ;	
   ;		tdot=(up.x*xls+up.y*yls+up.z*zls)/range1;
   ;	
	?debug	L 521
	fld	qword ptr [bp-230]
	fmul	qword ptr [bp-44]
	fld	qword ptr [bp-222]
	fmul	qword ptr [bp-52]
	fadd	
	fld	qword ptr [bp-214]
	fmul	qword ptr [bp-60]
	fadd	
	fdiv	qword ptr [bp-28]
	fstp	dword ptr [bp-4]
   ;	
   ;		xls =xls/range1;
   ;	
	?debug	L 522
	fld	qword ptr [bp-44]
	fdiv	qword ptr [bp-28]
	fstp	qword ptr [bp-44]
   ;	
   ;		yls =yls/range1;
   ;	
	?debug	L 523
	fld	qword ptr [bp-52]
	fdiv	qword ptr [bp-28]
	fstp	qword ptr [bp-52]
   ;	
   ;		zls =zls/range1;
   ;	
	?debug	L 524
	fld	qword ptr [bp-60]
	fdiv	qword ptr [bp-28]
	fstp	qword ptr [bp-60]
   ;	
   ;		range2=sqrt(pow(gpspos2.x-rec_pos_xyz.x-rpvt.xv,2)+
   ;	
	?debug	L 525
   ;	
   ;						pow(gpspos2.y-rec_pos_xyz.y-rpvt.yv,2)+
   ;						pow(gpspos2.z-rec_pos_xyz.z-rpvt.zv,2));
   ;	
	?debug	L 527
	fld	dword ptr DGROUP:s@+20
	sub	sp,8
	fstp	qword ptr [bp-266]
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz
	fsubr	qword ptr [bp-158]
	mov	ax,seg _rpvt
	mov	es,ax
	fsub	qword ptr es:_rpvt+32
	sub	sp,8
	fstp	qword ptr [bp-274]
	fwait	
	call	far ptr _pow
	add	sp,16
	fstp	tbyte ptr [bp-110]
	fld	dword ptr DGROUP:s@+20
	sub	sp,8
	fstp	qword ptr [bp-266]
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz+8
	fsubr	qword ptr [bp-150]
	mov	ax,seg _rpvt
	mov	es,ax
	fsub	qword ptr es:_rpvt+40
	sub	sp,8
	fstp	qword ptr [bp-274]
	fwait	
	call	far ptr _pow
	add	sp,16
	fld	tbyte ptr [bp-110]
	fadd	
	fstp	tbyte ptr [bp-110]
	fld	dword ptr DGROUP:s@+20
	sub	sp,8
	fstp	qword ptr [bp-266]
	mov	ax,seg _rec_pos_xyz
	mov	es,ax
	fld	qword ptr es:_rec_pos_xyz+16
	fsubr	qword ptr [bp-142]
	mov	ax,seg _rpvt
	mov	es,ax
	fsub	qword ptr es:_rpvt+48
	sub	sp,8
	fstp	qword ptr [bp-274]
	fwait	
	call	far ptr _pow
	add	sp,16
	fld	tbyte ptr [bp-110]
	fadd	
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _sqrt
	add	sp,8
	fstp	qword ptr [bp-36]
   ;	
   ;	
   ;		if ( tdot >= 1.00 ) satang=pi/2.0;
   ;	
	?debug	L 529
	fld	dword ptr [bp-4]
	fld1	
	fcompp
	fstsw	ax
	sahf	
	ja	short @1@13
	fld	qword ptr DGROUP:_pi
	fdiv	dword ptr DGROUP:s@+20
	jmp	short @1@16
@1@13:
   ;	
   ;		else if ( tdot <= -1.00 ) satang=-pi/2.0;
   ;	
	?debug	L 530
	fld	dword ptr [bp-4]
	fcomp	qword ptr DGROUP:s@+52
	fstsw	ax
	sahf	
	ja	short @1@15
	fld	qword ptr DGROUP:_pi
	fchs	
	fdiv	dword ptr DGROUP:s@+20
	jmp	short @1@16
@1@15:
   ;	
   ;		else  satang=asin(tdot);
   ;	
	?debug	L 531
	fld	dword ptr [bp-4]
	sub	sp,8
	fstp	qword ptr [bp-266]
	fwait	
	call	far ptr _asin
	add	sp,8
@1@16:
	fstp	dword ptr [bp-12]
	fwait	
   ;	
   ;	
   ;		xaz=east.x*xls+east.y*yls;
   ;	
	?debug	L 533
	fld	qword ptr [bp-206]
	fmul	qword ptr [bp-44]
	fld	qword ptr [bp-198]
	fmul	qword ptr [bp-52]
	fadd	
	fstp	qword ptr [bp-68]
   ;	
   ;		yaz=north.x*xls+north.y*yls+north.z*zls;
   ;	
	?debug	L 534
	fld	qword ptr [bp-182]
	fmul	qword ptr [bp-44]
	fld	qword ptr [bp-174]
	fmul	qword ptr [bp-52]
	fadd	
	fld	qword ptr [bp-166]
	fmul	qword ptr [bp-60]
	fadd	
	fstp	qword ptr [bp-76]
   ;	
   ;		if (xaz !=0.0 || yaz !=0.0) az=atan2(xaz,yaz);
   ;	
	?debug	L 535
	fld	qword ptr [bp-68]
	fldz	
	fcompp
	fstsw	ax
	sahf	
	jne	short @1@19
	fld	qword ptr [bp-76]
	fldz	
	fcompp
	fstsw	ax
	sahf	
	je	short @1@20
@1@19:
	fld	qword ptr [bp-76]
	sub	sp,8
	fstp	qword ptr [bp-266]
	fld	qword ptr [bp-68]
	sub	sp,8
	fstp	qword ptr [bp-274]
	fwait	
	call	far ptr _atan2
	add	sp,16
	fstp	dword ptr [bp-8]
	fwait	
	jmp	short @1@21
@1@20:
   ;	
   ;		else az=0.0;
   ;	
	?debug	L 536
	mov	dword ptr [bp-8],large 0
@1@21:
   ;	
   ;		result.x=gpspos1.x;
   ;	
	?debug	L 537
	fld	qword ptr [bp-134]
	fstp	dword ptr [bp-242]
   ;	
   ;		result.y=gpspos1.y;
   ;	
	?debug	L 538
	fld	qword ptr [bp-126]
	fstp	dword ptr [bp-238]
   ;	
   ;		result.z=gpspos1.z;
   ;	
	?debug	L 539
	fld	qword ptr [bp-118]
	fstp	dword ptr [bp-234]
   ;	
   ;		result.elevation=satang;
   ;	
	?debug	L 540
	fwait	
	mov	eax,dword ptr [bp-12]
	mov	dword ptr [bp-250],eax
   ;	
   ;		result.azimuth  =az;
   ;	
	?debug	L 541
	mov	eax,dword ptr [bp-8]
	mov	dword ptr [bp-254],eax
   ;	
   ;		result.doppler  =(range1-range2)/lambda; // changed to lambda
   ;	
	?debug	L 542
	fld	qword ptr [bp-28]
	fsub	qword ptr [bp-36]
	fdiv	qword ptr DGROUP:_lambda
	fstp	dword ptr [bp-246]
	fwait	
@1@22:
   ;	
   ;	   }
   ;	//	gotoxy(1,24);
   ;	//	printf("satfind->");
   ;		return(result);
   ;	
	?debug	L 546
	lea	si,word ptr [bp-254]
	mov	ax,ss
	les	di,dword ptr [bp+6]
	push	ds
	mov	ds,ax
	mov	cx,12
	rep 	movsw	
	pop	ds
	mov	dx,word ptr [bp+8]
	mov	ax,word ptr [bp+6]
   ;	
   ;	}
   ;	
	?debug	L 547
	pop	di
	pop	si
	leave	
	ret	
	?debug	C E318046563656618001E01
	?debug	C E31A02746D12001E05
	?debug	C E319000400161A00
	?debug	C E31B0673617476697318001E0F
	?debug	C E31C000400161B00
	?debug	C E60674696D655F730F02A4FF000003676D741902+
	?debug	C ACFF0000046A645F6D040402000006726573756C+
	?debug	C 741B0202FF000002757018021AFF000004656173+
	?debug	C 74180232FF0000056E6F72746818024AFF000007+
	?debug	C 677073706F7332180262FF000007677073706F73+
	?debug	C 3118027AFF0000056A645F79720602B0FF000003+
	?debug	C 79617A0F02B4FF00000378617A0F02BCFF000003+
	?debug	C 7A6C730F02C4FF000003796C730F02CCFF000003+
	?debug	C 786C730F02D4FF00000672616E6765320F02DCFF+
	?debug	C 00000672616E6765310F02E4FF00000C616C6D61+
	?debug	C 6E61635F646174650E02ECFF000008616C6D5F74+
	?debug	C 696D650E02F0FF000006736174616E670E02F4FF+
	?debug	C 000002617A0E02F8FF00000474646F740E02FCFF+
	?debug	C 00000672657475726E1C0A060000000169020A0A+
	?debug	C 000000
	?debug	E
	?debug	E
@satfind$qc	endp
	assume	cs:GPSFUNCS_TEXT,ds:DGROUP
@satpos_almanac$qfc	proc	far
   ;	
   ;	ecef satpos_almanac( float time, char n)
   ;	
	?debug	L 581
	?debug	B
	push	bp
	mov	bp,sp
	sub	sp,114
	push	si
	push	di
	?debug	C E6016E020A0E0000000474696D650E0A0A000000
	?debug	B
	cmp	word ptr __stklen,sp
	ja	short @2@3
	call	far ptr F_OVERFLOW@
@2@3:
   ;	
   ;	{
   ;	      double ei,ea,diff,r,ta,la,aol,xp,yp,d_toa;
   ;	      ecef result;
   ;	/*
   ;	      MA IS THE ANGLE FROM PERIGEE AT TOA
   ;	*/
   ;	//	gotoxy(1,24);
   ;	//	printf("->satpos_almanac");  
   ;	
   ;		d_toa = time - gps_alm[ n ].toa;
   ;	
	?debug	L 591
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx+32]
	fsubr	dword ptr [bp+10]
	fstp	qword ptr [bp-80]
   ;	
   ;	   if (d_toa>302400.0) d_toa=d_toa-604800.0;
   ;	
	?debug	L 592
	fld	qword ptr [bp-80]
	fcomp	dword ptr DGROUP:s@+60
	fstsw	ax
	sahf	
	jbe	short @2@5
	fld	qword ptr [bp-80]
	fsub	dword ptr DGROUP:s@+64
	jmp	short @2@7
@2@5:
   ;	
   ;	   else if (d_toa<-302400.0)d_toa=d_toa+604800.0;
   ;	
	?debug	L 593
	fld	qword ptr [bp-80]
	fcomp	qword ptr DGROUP:s@+68
	fstsw	ax
	sahf	
	jae	short @2@8
	fld	qword ptr [bp-80]
	fadd	dword ptr DGROUP:s@+64
@2@7:
	fstp	qword ptr [bp-80]
	fwait	
@2@8:
   ;	
   ;		ei = gps_alm[ n ].ma + d_toa * gps_alm[ n ].w;
   ;	
	?debug	L 594
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx+28]
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx]
	fmul	qword ptr [bp-80]
	fadd	
	fstp	qword ptr [bp-8]
   ;	
   ;	   ea = ei;
   ;	
	?debug	L 595
	fld	qword ptr [bp-8]
	fstp	qword ptr [bp-16]
	fwait	
@2@9:
   ;	
   ;		do
   ;	      {
   ;	         diff=(ei-(ea-gps_alm[n].ety*sin(ea)))/(1.-gps_alm[n].ety*cos(ea));
   ;	
	?debug	L 598
	fld	qword ptr [bp-16]
	sub	sp,8
	fstp	qword ptr [bp-126]
	fwait	
	call	far ptr _sin
	add	sp,8
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fmul	dword ptr es:_gps_alm[bx+4]
	fsubr	qword ptr [bp-16]
	fsubr	qword ptr [bp-8]
	fstp	tbyte ptr [bp-90]
	fld	qword ptr [bp-16]
	sub	sp,8
	fstp	qword ptr [bp-126]
	fwait	
	call	far ptr _cos
	add	sp,8
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fmul	dword ptr es:_gps_alm[bx+4]
	fld1	
	fsubr	
	fld	tbyte ptr [bp-90]
	fdivr	
	fstp	qword ptr [bp-24]
   ;	
   ;	         ea=ea+diff;
   ;	
	?debug	L 599
	fld	qword ptr [bp-16]
	fadd	qword ptr [bp-24]
	fstp	qword ptr [bp-16]
	fwait	
   ;	
   ;	      } while (fabs(diff) > 1.0e-6);
   ;	
	?debug	L 600
	fld	qword ptr [bp-24]
	fabs	
	fcomp	qword ptr DGROUP:s@+76
	fstsw	ax
	sahf	
	ja	@2@9
   ;	
   ;	/*
   ;	      EA IS THE ECCENTRIC ANOMALY
   ;	*/
   ;	      if (gps_alm[n].ety != 0.0 )
   ;	
	?debug	L 604
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx+4]
	fldz	
	fcompp
	fstsw	ax
	sahf	
	je	@2@13
   ;	
   ;		ta=atan2(sqrt(1.-pow(gps_alm[n].ety,2))*sin(ea),cos(ea)-gps_alm[n].ety);
   ;	
	?debug	L 605
	fld	qword ptr [bp-16]
	sub	sp,8
	fstp	qword ptr [bp-126]
	fwait	
	call	far ptr _cos
	add	sp,8
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fsub	dword ptr es:_gps_alm[bx+4]
	sub	sp,8
	fstp	qword ptr [bp-126]
	fld	dword ptr DGROUP:s@+20
	sub	sp,8
	fstp	qword ptr [bp-134]
	fwait	
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx+4]
	sub	sp,8
	fstp	qword ptr [bp-142]
	fwait	
	call	far ptr _pow
	add	sp,16
	fld1	
	fsubr	
	sub	sp,8
	fstp	qword ptr [bp-134]
	fwait	
	call	far ptr _sqrt
	add	sp,8
	fstp	tbyte ptr [bp-90]
	fld	qword ptr [bp-16]
	sub	sp,8
	fstp	qword ptr [bp-134]
	fwait	
	call	far ptr _sin
	add	sp,8
	fld	tbyte ptr [bp-90]
	fmul	
	sub	sp,8
	fstp	qword ptr [bp-134]
	fwait	
	call	far ptr _atan2
	add	sp,16
	fstp	qword ptr [bp-40]
	jmp	short @2@14
@2@13:
   ;	
   ;	      else
   ;		ta=ea;
   ;	
	?debug	L 607
	fld	qword ptr [bp-16]
	fstp	qword ptr [bp-40]
@2@14:
	fwait	
   ;	
   ;	/*
   ;	      TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
   ;	*/
   ;	      r=pow(gps_alm[n].sqa,2)*(1.-gps_alm[n].ety*cos(ea));
   ;	
	?debug	L 611
	fld	qword ptr [bp-16]
	sub	sp,8
	fstp	qword ptr [bp-126]
	fwait	
	call	far ptr _cos
	add	sp,8
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fmul	dword ptr es:_gps_alm[bx+4]
	fld1	
	fsubr	
	fstp	tbyte ptr [bp-90]
	fld	dword ptr DGROUP:s@+20
	sub	sp,8
	fstp	qword ptr [bp-126]
	fwait	
	mov	al,byte ptr [bp+14]
	cbw	
	imul	ax,ax,74
	mov	dx,seg _gps_alm
	mov	bx,ax
	mov	es,dx
	fld	dword ptr es:_gps_alm[bx+16]
	sub	sp,8
	fstp	qword ptr [bp-134]
	fwait	
	call	far ptr _pow
	add	sp,16
	fld	tbyte ptr [bp-90]
	fmul	
	fstp	qword ptr [bp-32]
   ;	
   ;	/*
   ;	      R IS THE RADIUS OF SATELLITE ORBIT AT TIME T