! ! File: essentialFunc.f ! Author: Andria.Bilich ! lines stolen from ABtimetrans.f, gpsGeneral.f and gpsConstants.h ! ! Created on October 27, 2010, 4:33 PM ! c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= subroutine orbit (staxyz, tc, NAV, prn, recf, vecf, svclk, . ivel, ierr, navc) c account for Sagnac effect: c you need satellite coordinates at transmit time and c receiver coordinates at reception time to model the range. c the equation to determine this isn't linear, so you iterate. c-- INPUT PARAMETERS -------------------------------------------------- c staxyz(3) station a priori coordinates m c tc(2) epoch of observation [sec week] c prn PRN number of satellite c NAV() matrix containing broadcast navigation parameters c ivel flag for computing velocity (1=get velocity) c ierr flag that there was an error with orbits (0=ok) c c-- OUTPUT PARAMETERS ------------------------------------------------- c recf(3) satellite position, ECEF frame m c vecf(3) satellite velocity, ECEF frame m/s c svclk satellite clock correction m c c-- INTERNAL CONSTANTS ------------------------------------------------ c c-- INTERNAL VARIABLES ------------------------------------------------ c ********************************************* implicit none c include 'gpsConstants.h' integer MAXSAT, MAXEPH, MAXORB real*8 OmDotEarth, clight parameter (MAXSAT = 50) parameter (MAXEPH = 50) parameter (MAXORB = 23) parameter ( OmDotEarth = 7.2921151467E-5 ) parameter ( clight = 0.299792458e9 ) integer prn, jj, ierr, ivel, navc real*8 tc(2), tin(2), recf(3), vecf(3), staxyz(3), svclk real*8 toffset, omeg, xnew, ynew real*8 NAV(MAXEPH,MAXSAT,MAXORB) c ********************************************* c initial value of time offset toffset = 0.070 c call bceph(tc,NAV,prn,recf,vecf,svclk,ivel,ierr) c print *, (recf(jj), jj=1,3) c three iterations should do it, 4 is overkill tin(2) = tc(2) do jj = 1, 3 c print *, 'time offset = ', toffset tin(1) = tc(1)-toffset call bceph2(tin,NAV,navc,prn,recf,vecf,svclk,ivel,ierr) c call bceph(tin,NAV,prn,recf,vecf,svclk,ivel,ierr) if (ierr.ne.0) goto 111 omeg = -OmDotEarth*toffset xnew = recf(1)*dcos(omeg) - recf(2)*dsin(omeg) ynew = recf(1)*dsin(omeg) + recf(2)*dcos(omeg) toffset = dsqrt( (xnew - staxyz(1))**2 + . (ynew - staxyz(2))**2 + . (recf(3)-staxyz(3))**2 )/clight enddo recf(1) = xnew recf(2) = ynew 111 continue return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= subroutine azel(azimuth, elev, station, East, North, . Staup, XYZ) c c INPUTS: c station(3) cartesian station position m real c XYZ(3) cartesian satellite position m real c East, North, Up unit vectors [unitless] real c OUTPUTS: c azimuth degrees real c elev degrees real c ---------------------------------------------------- implicit none c include 'gpsConstants.h' real*8 pi parameter ( pi = 3.1415926535898d0 ) real*8 elev, azimuth real*8 station(3), XYZ(3), East(3), North(3), Staup(3) double precision sta2sat(3), Mstation, Msat, . sta2sat_N, sta2sat_E, UdotS, zenith, range c ---------------------------------------------------- c ... find azimuth/elevation of satellite relative c ... to the reference station (station(3)) c print *, X, Y, Z c print *, station(1), station(2), station(3) sta2sat(1) = dble(XYZ(1) - station(1)) sta2sat(2) = dble(XYZ(2) - station(2)) sta2sat(3) = dble(XYZ(3) - station(3)) c print *, sta2sat(1), sta2sat(2), sta2sat(3) range = dsqrt(sta2sat(1)**2+ sta2sat(2)**2 +sta2sat(3)**2) c print *,' ', range c ... azimuth of satellite from station: c ... get components of 'sta2sat' in ENV frame: sta2sat_E = East(1)*sta2sat(1) + East(2)*sta2sat(2) . + East(3)*sta2sat(3) sta2sat_N = North(1)*sta2sat(1) + North(2)*sta2sat(2) . + North(3)*sta2sat(3) c atan2(X,Y) == tan-1(X/Y) azimuth = sngl(datan2(sta2sat_E, sta2sat_N)*180/pi) if (azimuth .lt. 0) then azimuth = 360 + azimuth endif c ... elevation angle calculation: Mstation = dsqrt(Staup(1)**2 + Staup(2)**2 + . Staup(3)**2) Msat = dsqrt(sta2sat(1)**2 + sta2sat(2)**2 + . sta2sat(3)**2) UdotS = Staup(1)*sta2sat(1) + Staup(2)*sta2sat(2) + . Staup(3)*sta2sat(3) zenith = dacos(UdotS/(Mstation*Msat))*180/pi elev = sngl(90 - zenith) return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= c from xenon:~/Src/Atime.f, "rnxtime" subroutines subroutine ymdhms2gpsws(YR, MO, DAY, HR, MIN, SEC, + GPS_SEC, GPS_WEEK) c convert year month day hour minute seconds c into seconds of GPS week and GPS week c *************************************************** real*8 GPS_SEC, GPS_WEEK c double precision GPS_SEC, GPS_WEEK character*20 TSTRING integer YR, MO, DAY, HR, MIN, SEC real*8 UT, MIN2, HR2 double precision JD c *************************************************** c convert 2-digit year into 4-digits if (YR .lt. 80) then YR = YR + 2000 else YR = YR + 1900 endif if (MO .gt. 2) then YEAR = YR MONTH = MO else YEAR = YR - 1 MONTH = MO + 12 endif HR2 = real(HR) MIN2 = real(MIN) UT = HR2 + (MIN2/60) + (real(SEC)/3600) c print *, HR2, MIN2, real(SEC) JD = int(365.25*YEAR) + int(30.6001*(MONTH+1)) + DAY + . (UT/24) + 1720981.5 GPS_WEEK = idint((JD - 2444244.5)/7) GPS_SEC = ( ((JD-2444244.5)/7) - GPS_WEEK )*7*24*3600 return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= c from gpsGeneral.f subroutine xyzllh( xyz, llh, iflag) c c --> stolen from "geoxyz", ~kristine/Src/Mythesis3/xyz2llh.f <-- c c Purpose: c Convert geodetic curvilinear coordinates to geocentric Cartesian c coordinates and vice versa c c Input: c iflag = 1 convert lat/long/height to XYZ integer c = 2 convert XYZ to lat/long/height c c Input/Output: c llh : geodetic latitude and longitude (degrees), real*8 c height above the reference ellipsiod (meters) c xyz : geocentric Cartesian coordinates (meters) real*8 c c Notes: c Currently assumes the WGS84 reference ellipsoid; c Clarke 1866 ellipsoid with approximate translation parameters c for NAD27 are commented. c Cartesian to geodetic conversion uses an iterative scheme c valid to the millimeter level. c implicit none c include 'gpsConstants.h' real*8 pi, twopi integer iflag real*8 alat,along,hght,x,y,z, xyz(3) real*8 semi,finv,tx,ty,tz, b, llh(3) real*8 f,e2,curvn,sqr,alat0,cutoff real*8 sinlat,coslat,sinlon,coslon, ep2 parameter ( pi = 3.1415926535898d0 ) parameter ( twopi = pi+pi ) c NAD27: Clarke 1866 ellipsoid, approximate translations c data semi,finv,tx,ty,tz/ 6378206.4d0, 294.9786982d0, c 1 -12.010d0, 162.970d0, 189.740d0/ c c ------------------------------------------------------------- c NAD83 = WGS84 ellipsoid semi = 6378137.d0 finv = 298.257222101d0 c finv = 298.2572235630 tx = 0.0 ty = 0.0 tz = 0.0 c print*, alat, along, hght, x, y ,z , iflag f= 1.d0/finv b = -f*semi +semi ep2 = (semi*semi - b*b)/b/b e2= 2.d0*f - f*f if( iflag.eq.1) then alat = llh(1)*pi/180 along = llh(2)*pi/180 hght = llh(3) sinlat= dsin(alat) coslat= dcos(alat) sinlon= dsin(along) coslon= dcos(along) curvn= semi/(dsqrt(1.d0-e2*sinlat*sinlat)) xyz(1) = (curvn+hght)*coslat*coslon + tx xyz(2) = (curvn+hght)*coslat*sinlon + ty xyz(3) = (curvn*(1.d0-e2)+hght)*sinlat + tz else x= xyz(1) - tx y= xyz(2) - ty z= xyz(3) - tz along= datan2(y,x) if( along.lt.0d0 ) along=along + twopi c starting value for latitude iteration sqr= dsqrt(x*x+y*y) alat0= datan2(z/sqr,1.d0-e2) alat= alat0 40 sinlat= dsin(alat) curvn= semi/(dsqrt(1.d0-e2*sinlat*sinlat)) alat= datan2((z+e2*curvn*sinlat),sqr) c iterate to millimeter level if( dabs(alat-alat0).lt.1.d-10) goto 30 alat0= alat goto 40 30 continue cutoff= 80.d0*twopi/360.d0 if(alat.le.cutoff) then hght= (sqr/dcos(alat))-curvn else hght= z/dsin(alat)-curvn+e2*curvn endif llh(1) = alat*180/pi llh(2) = along*180/pi llh(3) = hght endif return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= subroutine llh2locenv(NORTH, EAST, UP, inLAT, inLONG) c rotate station lat/long/height into local east/north frame c c VARIABLE DEFINITION UNITS DATA_TYPE c INPUT c inlat latitude degrees real*8 c inlong longitude degrees real*8 c OUTPUT c north, east, up c unit vectors unitless real*8 implicit none c include 'gpsConstants.h' real*8 NORTH(3), EAST(3), UP(3), pi, twopi real*8 LAT, LONG, inlat, inlong parameter ( pi = 3.1415926535898d0 ) parameter ( twopi = pi+pi ) c ****************************** c lat = dble(inlat)*pi/180 c long = dble(inlong)*pi/180 lat = inlat*pi/180 long = inlong*pi/180 UP(1) = cos(Lat)*cos(Long) UP(2) = cos(Lat)*sin(Long) UP(3) = sin(Lat) c ... also define local east/north for station: NORTH(1) = -sin(Lat)*cos(Long) NORTH(2) = -sin(Lat)*sin(Long) NORTH(3) = cos(Lat) EAST(1) = -sin(Long) EAST(2) = cos(Long) EAST(3) = 0 return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= SUBROUTINE NEUXYZ(RLAT,RLON,DX,DY,DZ,DN,DE,DU) C c stolen from Gerry Mader's "antcal" routines: neuxyz.f c C SUBROUTINE TO CONVERT LOCAL COORDINATES DN,DE,DU (M) TO C A VECTOR DX,DY,DZ (M) AT POSITION RLAT,RLON (DEC. DEG.) c ---------------------------------------------------- IMPLICIT REAL*8 (A-H,O-Z) DTR=1.D0/57.2957795D0 c ---------------------------------------------------- SLAT=DTR*RLAT SLON=DTR*RLON DX=-DN*DSIN(SLAT)*DCOS(SLON) * -DE*DSIN(SLON)+DU*DCOS(SLAT)*DCOS(SLON) DY=-DN*DSIN(SLAT)*DSIN(SLON) * +DE*DCOS(SLON)+DU*DCOS(SLAT)*DSIN(SLON) DZ=+DN*DCOS(SLAT)+DU*DSIN(SLAT) RETURN END c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= subroutine rangecalc(recf, stapos, delrange, georange) c ---------------------------------------------------- implicit none real*8 georange, delrange(3), stapos(3) double precision recf(3) integer i c ---------------------------------------------------- do i = 1,3 delrange(i) = recf(i) - stapos(i) enddo georange = dsqrt(delrange(1)**2 + delrange(2)**2 . + delrange(3)**2) return end c -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= subroutine gpsws2ymdhms(gps_week, sec_of_week, $ year,month,mday,hour,minute,second) implicit none cc from GPS Toolbox, Remondi date conversion codes cc (renamed from "gps_to_ymdhms") integer*4 gps_week,year,month,mday,hour,minute real*8 sec_of_week, second integer*4 jan61980, jan11901 real*8 sec_per_day integer*4 guess, more, yday, mjd, days_fr_jan11901 integer*4 leap_month_day, delta_yrs, num_four_yrs integer*4 regu_month_day, days_left, years_so_far real*8 fmjd dimension regu_month_day(13) dimension leap_month_day(13) data regu_month_day / 0, 31, 59, 90, 120, 151, 181, 212, $ 243, 273, 304, 334, 365 / data leap_month_day / 0, 31, 60, 91, 121, 152, 182, 213, $ 244, 274, 305, 335, 366 / c January 6, 1980 00:00 = GPS standard epoch jan61980 = 44244 jan11901 = 15385 sec_per_day = 86400.0d0 mjd = gps_week*7 + sec_of_week/sec_per_day + jan61980 fmjd = dmod(sec_of_week, sec_per_day)/sec_per_day c write(6,100) mjd, fmjd c100 format('mjd, fmjd:', i9,2x,f16.14) days_fr_jan11901 = mjd - jan11901 num_four_yrs = days_fr_jan11901/1461 years_so_far = 1901 + 4*num_four_yrs days_left = days_fr_jan11901 - 1461*num_four_yrs delta_yrs = days_left/365 - days_left/1460 year = years_so_far + delta_yrs yday = days_left - 365*delta_yrs + 1 hour = fmjd*24.0d0 minute = fmjd*1440.0d0 - hour*60.0d0 second = fmjd*86400.0d0 - hour*3600.0d0 - minute*60.0d0 c write(6,'('' year is:''i9)') year c write(6,'('' day-of-year is:''i9)') yday guess = yday*0.0320d0 + 1 more = 0 if( mod(year,4) .eq. 0 ) then if( (yday - leap_month_day(guess+1)) .gt. 0 )more = 1 month = guess + more mday = yday - leap_month_day(guess+more) else if( (yday - regu_month_day(guess+1)) .gt. 0 )more = 1 month = guess + more mday = yday - regu_month_day(guess+more) endif c write(6,'('' month is:''i9)') month c write(6,'('' day-of-month is:''i9)') mday return end