program readegi8 ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c R E A D E G I c c program to read H764G raw data as logged by "egicon" c c input: c c egifile c subw, dt, t1, t2, lvel c c where c c subw - wanted record type (17 blended GPS/INS, 29 free INS, -29: free INS before 2005) c dt - time spacing of output data (sec), 0 = all data c t1,t2 - wanted output epochs process (dechr) c lvel - true if outputfile with velocity/acc and wander azim wanted c c outputfiles (2nd only if lvel true): c c readegi.coo - thr, lat, lon, h, pitch, roll, true heading, vzint c readegi.vel - thr, vx, vy, vz, accx, accy, accz, alpha c (test.vel - thr, h, vzint, vz, acc, dv/dz) c c NB: time, velocity and acc data are averaged for dt > 0 to limit noise c units: m, m/s, m/s**2 and degrees. thr is the GPS time in dechr c c (c) Rene Forsberg/National Survey and Cadastre (KMS) c 8 jan 2001 c updated 26 jun 2001 c updated with integrated velocities aug 3, 2001, rf c VERSION TO GET EGI TIME TAG FROM GPS word 2 c UPDATED BY K.KELLER MAY 2003 c ----- to run past midnight, avo Aug 05 c info about EGI data: 'EGIcon.doc' June 20, 2000 by Kim Woelders c EGI time tag error fixes and cleanup, RF jan 08 c Correction for EGI upgrade 2005 (change of acc and v) and data gaps, RF jan 2012 c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc implicit real*8(a-h,o-z) logical lalign,lstart,lout,louth,lvel,lold character*72 ifile character*1 tchar character*3 mode2char integer*1 iyr,imon,iday,ihr,imin,isec,type,sub,len,subw integer*2 iswap,hdr(12),w(32),dhdr(4),datseq,ms,fom,mode,mode2 integer*2 w8(29) integer*4 hdrseqn,iutc8,iutc,imw2 integer*1 butc(4) equivalence (iutc,butc) equivalence (hdr(5),hdrseqn),(hdr(8),datseq),(hdr(12),ms) c c program input c ------------- c read(*,'(a)') ifile read(*,*) subw, dt, t1, t2, lvel c lold = (subw.eq.-29.or.subw.eq.17) subw = abs(subw) if (.not.(subw.eq.17.or.subw.eq.29)) stop '*** subw wrong' c write(*,5) dt,ifile 5 format(' --- READEGI ---'/ .' data output at:',f5.2,' sec, EGI file: ',a36) if (lold) write(*,*) '--- Pre-2005 EGI data format for subw=29' open(10,file=ifile,form='unformatted',access='direct',recl=2) open(20,file='readegi.coo',status='unknown') c if (lvel) open(21,file='readegi.vel',status='unknown') if (lvel) open(22,file='test.vel',status='unknown') c c constants c --------- c hdrsync = 5A A5 hex, corresponds to signed integer -23206 hdrsync = -23206 max4 = 2147483647 max2 = 32767 radeg = 180/3.141592653589d0 c direction cosines - factor 2 seems necessary? fakc = 2.d0/max4 c longitude faklon = 180.d0/max4 c roll,heading,pitch fakr = 180.d0/max2 c altitude faka = 131068.d0*0.3048/max2 c speed convert from ft/s to m/s fakv_old = 8192.d0*0.3048/max4 fakv = 8192.d0*0.3048/max2 c acc fakacc_old = 1023.97d0*0.3048/max2 fakacc = 1023.97d0*0.3048/max4 c utc faku = 131072.d0/max4 c egi time tag c faktt = 4.194240d0/(max2*2.0d0) c alternative factor: faktt = 2.0971520d0/max2 c counters: irec is input record counter c nrec is record counter c nsub counts number of read wanted subrecords c nsubo counts number of output wanted subrecords c maxt = 36.0d0 irec = 0 nrec = 0 nsub = 0 nsubo = 0 nsub8 = 0 n8 = 0 n6 = 0 n16 = 0 n17 = 0 n29 = 0 lstart = .true. lalign = .true. thrp = -9999 thrup = -9999 dthr = (dt-0.001)/3600 dthrmax = 0 nsum = 0 tsum = 0 vxsum = 0 vysum = 0 vzsum = 0 axsum = 0 aysum = 0 azsum = 0 utcp = 0 vzint = 0 utc8p = 0 tt8p = 0 ttbias = 0 ttudif = 0 c c ----------------------------- c main loop reentry c read header block (in record) c ----------------------------- c 10 do j = 1,12 irec = irec+1 read(10,rec=irec,err=90) hdr(j) enddo c c 2 lines added to to handle midnight problem, avo Aug 05 call split(hdr(10),iday,ihr) if (n17.lt.5.and.n29.lt.5) ifirstday = iday c c check if header record is correct. If not skip forward to find header c if (hdr(1).ne.hdrsync.or.hdr(2).ne.hdrsync) then write(*,*) '*** rechdr not ok, irec=',irec,', hdr=', . hdr(1),hdr(2) 12 irec = irec+1 read(10,rec=irec,err=90) hdr(1) if (hdr(1).ne.hdrsync) goto 12 irec = irec-1 goto 10 endif c call split(hdr(9),iyr,imon) call split(hdr(10),iday,ihr) call split(hdr(11),imin,isec) c changed to handle midnight problem, avo Aug 05 thr=(iday-ifirstday)*24.d0+ihr+imin/60.d0+isec/3600.d0+ms/3600000.d0 if (thr.gt.maxt) thr = thr - 24.0d0 if (lstart) then ihr1 = ihr imin1 = imin thr1 = thr thrp = thr write(*,14) iyr+2000,imon,iday,ifirstday,subw 14 format(/' hdr date: ',i4,i3,i3,' firstday',i3, . ', wanted subadr:',i3) endif c c read data block header (first 4 bytes) c -------------------------------------- c c format: 1: dev + type c 2: subw + len c 3: TTBC c 4: status c for more info see 'EGIcon.doc' by Kim Woelders c do j = 1,4 irec = irec+1 read(10,rec=irec,err=90) dhdr(j) enddo type = dhdr(1)/256 tchar='T' if (type.ne.1) tchar='R' call split(dhdr(2),sub,len) c c data header output on screen - occasionally c c counting records c nrec = nrec+1 if (sub.eq.8) n8 = n8+1 if (sub.eq.6) n6 = n6+1 if (sub.eq.16) n16 = n16+1 if (sub.eq.17) n17 = n17+1 if (sub.eq.29) n29 = n29+1 lout = (mod(nsub,30000).eq.1) louth = (nrec.lt.20) c if (louth) write(*,15) hdrseqn,datseq, .ihr,imin,isec,ms,tchar,sub,len,irec 15 format(' hdr',i8,' dat',i8, .' time ',3i3,'.',i3,' sub ',a1,i3,' len ',i2, .' rec ',i8) c c skip unwanted records - jump back for new record c if (sub.ne.subw.and.sub.ne.8) then irec = irec+len goto 10 endif c IF (SUB.EQ.17.OR.SUB.EQ.29) THEN c c decode wanted subadr data c -------------------------- c nsub = nsub+1 do j= 1,len irec = irec+1 read(10,rec=irec,err=90) w(j) enddo c c sub 17 or 29 c cnexz = long(w(21),w(22))*fakc rlat = asin(cnexz)*radeg rlon = long(w(23),w(24))*faklon alt = iswap(w(25))*faka roll = iswap(w(10))*fakr pitch = iswap(w(11))*fakr hdg = iswap(w(12))*fakr c iutc = long(w(27),w(28)) c utc = iutc*faku c tt1 = real(iswap256(w(2))) c ttp = tt tt = tt1*faktt c if (lold) then vz = long(w(7),w(8))*fakv_old else vz = iswap(w(16))*fakv endif mode2 = iswap(w(29)) if (lvel) then azim = iswap(w(9))*fakr if (lold) then vx = long(w(3),w(4))*fakv_old vy = long(w(5),w(6))*fakv_old ax = iswap(w(14))*fakacc_old ay = iswap(w(15))*fakacc_old az = iswap(w(16))*fakacc_old else ax = long(w(3),w(4))*fakacc ay = long(w(5),w(6))*fakacc az = long(w(7),w(8))*fakacc vx = iswap(w(14))*fakv vy = iswap(w(15))*fakv endif c if (dt.gt.0) then nsum = nsum+1 tsum = tsum+thru vxsum = vxsum+vx vysum = vysum+vy vzsum = vzsum+vz axsum = axsum+ax aysum = aysum+ay azsum = azsum+az endif endif c c update time info c internal egi clock reset after 4.2 sec c if (tt-ttp.lt.0) ttbias = ttbias + 4.19436800d0 te = tt + ttbias utc = te + ttudif c write(*,181) tt,te c181 format(' test tt te :',2f12.4) c ELSEIF (SUB.EQ.8) THEN c c message 8 is GPS data: use this to update ttudif c UTC time tag: word (19/20) c and EGI time tag: word 2 (time of validity) c tt8p = tt8 utc8p = utc8 nsub8 = nsub8+1 do j = 1, len irec = irec+1 read(10,rec=irec) w8(j) enddo c iutc8 = long(w8(19),w8(20)) utc8 = iutc8*faku tt8 = real(iswap256(w8(2))) tt8 = tt8*faktt if (tt.lt.tt8) tt8 = tt8 - 4.19436800d0 te8 = ttbias + tt8 ttudif = utc8-te8 c write(*,18) tt8,te8,utc8,ttudif c18 format(' test tt8 te8 utc8 dif:',4f12.4) c c warning for time gaps c tgap = utc8-utc8p if (n8.gt.1.and.tgap.gt.3.5) then ihrg = utc8/3600 iming = (utc8-ihrg*3600)/60 isecg = utc8-ihrg*3600-iming*60 write(*,17) utc8/3600,ihrg,iming,isecg,tgap 17 format(' major gps time gap: ',f9.5,i4,i3.2,i3.2', sec:',f7.3) endif goto 10 ENDIF c thr = utc/3600.d0 thru = utc/3600.0d0 + 24.0d0*(iday - ifirstday) c c integrate vertical velocity c if (nsub8.ge.2) then if (nsub8.eq.2) vzint=alt if (thru.gt.maxt) thru = thru - 24.0d0 dtu = utc-utcp if (dtu.gt.86400) dtu = dtu - 86400.0d0 if (dtu.lt.-86400) dtu = dtu + 86400.0d0 if (thru.lt.t1.or.utcp.le.0) then vzint = alt else vzint = vzint + dtu*vz endif endif utcp = utc c c output data on screen and file c ------------------------------ c if (lout) then mode = iswap(w(1)) fom = iswap(w(26)) c c decode fom and mode2 c ifom3 = mod(fom,16) ifom2 = mod(fom/16,16) ifom1 = mod(fom/256,16) mode2char = 'unk' if (mod(mode2/16384,2).eq.1) mode2char = 'sby' if (mod(mode2/8192,2).eq.1) mode2char = 'ala' if (mod(mode2/4096,2).eq.1) mode2char = 'sha' if (mod(mode2/2048,2).eq.1) mode2char = 'alg' if (mod(mode2/128,2).eq.1) mode2char = 'nav' if (mod(mode2,2).eq.1) mode2char = 'nrf' c if (thru.gt.t1) write(*,20) thru,rlat,rlon,alt,pitch,roll,hdg, . mode2char,ifom1,ifom2,ifom3,mode,ttudif 20 format(' ',f9.6,f10.5,f11.5,f8.1,2f6.1,f7.1,' ',a3,' ', . 3i1,i5,f9.1) endif c c output on files - position, pitch, roll, hdg c c to avoid double pos at same time if ((thru-thrup).le.0.000001) goto 10 c if (thr-thrp.gt.dthr.and.thru.gt.thrup) then if (thru.gt.t1) . write(20,21) thru,rlat,rlon,alt,pitch,roll,hdg,vzint 21 format(' ',f10.7,f12.7,f13.7,f9.1,2f8.3,f9.3,1x,f11.2) c c velocity/acc output - average c if (lvel) then alpha = azim-hdg if (alpha.lt.-180) alpha = alpha+360 if (alpha.gt.180) alpha = alpha-360 c if (dt.gt.0) then if (nsum.ge.1) then thru = tsum/nsum vx = vxsum/nsum vy = vysum/nsum vz = vzsum/nsum ax = axsum/nsum ay = aysum/nsum az = azsum/nsum nsum = 0 tsum = 0 vxsum = 0 vysum = 0 vzsum = 0 axsum = 0 aysum = 0 azsum = 0 endif endif c if (thru.gt.t1) . write(21,22) thru,vx,vy,vz,ax,ay,az,azim-hdg 22 format(' ',f10.7,2f13.5,f11.5,3f9.4,f8.2) c test output if (thru.gt.t1) then write(22,22) thru,alt,vzint,vz,az-9.82,testacc if (dt.eq.0) then testacc = 0 else testacc = (vz-vzp)/dt endif vzp = vz endif endif c if (thr-thrp.gt.dthrmax.and.thru.gt.t1) then dthrmax = thr-thrp thrmax = thr endif thrp = thr thrup = thru nsubo = nsubo+1 endif c c end of record loop c ------------------ c if (lalign) then if (mode2.eq.128) then lalign = .false. write(*,*) write(*,*) . ' thr lat lon h pitch roll', . ' hdg FOM mode egi2utc' ihru2 = thru iminu2 = (thru-ihru2)*60 endif endif if (lstart) lstart = .false. if (thru.gt.t2) goto 90 goto 10 c 90 write(*,91) iyr+2000,imon,iday 91 format(/' Date: ',i4,i3,i3) write(*,92) ihr1,imin1,ihr,imin,(thr-thr1)*60,ihru2,iminu2 92 format(' Start eqi time ',i2,':',i2,', stop ',i2,':',i2, .', duration:',f7.2,' min'/ .' Start nav time ',i2,':',i2) write(*,93) nrec,nsub,nsubo,n6,n8,n16,n17,n29 93 format(' Total records read:',i8,', wanted type read:',i8, .', output:',i8/' No of recs subadr 6,8,16,17,29: ',5i8) write(*,94) nsub/(thr-thr1)/3600,dthrmax*3600,thrmax 94 format(' Average logging rate:',f5.1,' Hz, max data time break:' .,f8.2,' sec at ',f9.6) end c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c integer*2 function iswap(i) c --------------------------- c function swaps the two bytes in i c integer*2 i,k byte b(2),b1 equivalence(b,k) k = i b1 = b(1) b(1) = b(2) b(2) = b1 iswap = k return end c c ---------------------- integer*4 function iswap256(i) c ---------------------------- c converts two bytes to an unsigned integer, c it must be an integer*4 becuase of the missing sign! c integer*4 i,k byte b(4),b1 equivalence(b,k) k = i b1 = b(1) c b(1) = b(2)*256 b(1) = b(2) b(2) = b1 b(3) = 0 b(4) = 0 iswap256 = k return end c ---------------------- integer*4 function iswap16(i) c ---------------------------- c converts one byte to an unsigned integer, c it must be an integer*4 becuase of the missing sign! c integer*4 i,k byte b(4) equivalence(b,k) k = i c b1 = b(1) b(1) = b(2) b(2) = 0 b(3) = 0 b(4) = 0 iswap16 = k return end c ------------------------- c subroutine split(i,i1,i2) c ------------------------- c splits i into first and second byte c integer*2 i,k integer*1 b(2),i1,i2 equivalence(b,k) k = i i1 = b(1) i2 = b(2) return end c integer*4 function long(i1,i2) c ------------------------------ c combines two EGI words into one integer*4 c integer*2 i1,i2,j(2) integer*4 k equivalence(j,k) j(2) = iswap(i1) j(1) = iswap(i2) long = k return end