PROGRAM FastTest

C Program to read a fast tape file and print diag info


      IMPLICIT none

      INCLUDE "FastCom.ftni"

      INTEGER InLu, OutLu, ios, BufSize, i,j,PCnt,NzIdx
      REAL NzMax
      CHARACTER*64 InFile, OutFile

      PARAMETER (InLu=35)  ! arbitrary #, Out will be 33

C Do some setup stuff
      DO i = 1,10
        INE_Over1(i) = 0  ! init to no overrun
        INE_Over2(i) = 0
      END DO
      VzLast = -999.  ! flag for no last vert speed
      NzLast = 0.     ! init vert accel
      Time_Offset = 0  ! show that we haven't started data timing
      PCnt = 100


C ask for and open the Setup file
      CALL GetSetupFile(InFile)

      OPEN(InLu,FILE=InFile,form='unformatted',
     &     access='direct',recl=1,ERR=999,IOSTAT=ios)
      RecNum_com = 1  ! pointer into input file

      Init = .TRUE.  ! need to wait for taxi
      CALL ReadBuf(InLu,BufSize)  ! so we can enter the do loop

C Loop on each 1 sec record
      DO WHILE (BufSize.GT.0)

C Decode the pointers and starting loc for each data type
        CALL Decode_Sec

C Extract INE data, specifically 3D speeds and 3D attitude
        CALL Extract_INE
        CALL Cnvt_INE

C If we don't have 40 samples, skip this sec
        IF (INE_Cnt1.LT.40) GOTO 100

C We have enough decoded to check for movement
        IF (Init) THEN  ! still waiting for aircraft movement
          IF (SQRT(NVel(1)**2.+EVel(1)**2.).GT.30.) THEN  ! we're moving
            VSum1 = 0.0  ! init Vert vel from AVx
            VSum2 = 0.0
c diag test print
            write(*,'(9(10i4,/))') (IB(i), i=15,105)

c            CALL Create_OutFile(OutLu)  ! create & open an output file
c            CALL Init_Hdr(OutLu)  ! write hdr lines, do other init
            Init = .FALSE.  ! only do it once
          ENDIF
        ENDIF  ! done if we're in Init
        IF (.NOT.Init) THEN  ! we're moving, do everything


C Extract PQ and PS, then calc the corrected values & IAS
          CALL Extract_ADC(PQchan,PQoff,PQslp,PQarray)
          CALL Extract_ADC(PSchan,PSoff,PSslp,PSarray)
          CALL Extract_ADC(16,0.0,4.9031,AV1array)
          CALL Extract_ADC(15,0.0,4.9031,AV2array)
          CALL Calc_IAS

C Calculate the aircraft-referenced vert velocity, then Accel (Nz)
          CALL Accel2Vel(VSum1,AV1array,Vv1array)
          CALL Accel2Vel(VSum2,AV2array,Vv2array)
          CALL Calc_Vz
c          CALL Calc_Nz


c diag print
c          NzMax = 0.
c          DO i = 1,40
c            IF ( Abs(Nz(i)).GT.Abs(NzMax) ) THEN
c              NzMax = Nz(i)
c              NzIdx = i
c            ENDIF
c          END DO
c          IF (Abs(NzMax).GT.0.9) THEN
          WRITE(*,'(i2,":",i2.2,":",i2.2" Nv:"f6.1" Ev:"f6.1" Vv:"f5.1
     &             " Hdg:"f6.1" Ptch:"f5.1" Roll:"f5.1" Vacc1:"f6.2
     &             " Vacc2:"f6.2," Vv1:"f5.1" Vv2:"f5.1)') 
     &     IB(6),IB(7),IB(8),NVel(1),EVel(1),VVel(1),Hdg(1)*57.29,
     &     Pitch(1)*57.29,Roll(1)*57.29,AV1array(1),AV2array(1),
     &     Vv1array(1),Vv2array(1)
c          IF (NzIdx.LT.6) NzIdx = 6
c          IF (NzIdx.GT.35) NzIdx = 35
c          WRITE(*,'("  Vz:"11f7.2)') (Vz(i),i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("  Nz:"11f7.2)') (Nz(i),i=NzIdx-5,NzIdx+5)
c          WRITE(*,'(" Hdg:"11f7.2)') (Hdg(i)*57.29,i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("Ptch:"11f7.2)') (Pitch(i)*57.29,i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("Roll:"11f7.2)') (Roll(i)*57.29,i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("NVel:"11f7.2)') (NVel(i),i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("EVel:"11f7.2)') (EVel(i),i=NzIdx-5,NzIdx+5)
c          WRITE(*,'("VVel:"11f7.2)') (VVel(i),i=NzIdx-5,NzIdx+5)
c          ENDIF

          PCnt = PCnt -1
          IF (PCnt.LE.0) THEN
            write(*,'("Press enter to exit")')
            read(*,'(a)') i
            stop
          ENDIF

c          CALL Write1Sec(OutLu)  ! write 1 sec of 40 Hz data

        ENDIF

100     CALL ReadBuf(InLu,BufSize)  ! always try for next read
      END DO  ! drop through at EOF

      CLOSE(InLu)  ! clean up file access
      CLOSE(OutLu)

      STOP

999   WRITE(*,'("Error while opening file: ",a," IOSTAT="*)')
     & InFile, ios
      STOP

      END

      INCLUDE "GetSetupFile.ftni"
      INCLUDE "Create_OutFile.ftni"
      INCLUDE "ReadBuf.ftni"
      INCLUDE "Decode_Sec.ftni"
      INCLUDE "Extract_INE.ftni"
      INCLUDE "Cnvt_INE.ftni"
      INCLUDE "PairCheck.ftni"
      INCLUDE "LimitTest.ftni"
      INCLUDE "Extract_ADC.ftni"
      INCLUDE "Calc_IAS.ftni"
      INCLUDE "Calc_Vz.ftni"
      INCLUDE "Calc_Nz.ftni"
      INCLUDE "Init_Hdr.ftni"
      INCLUDE "Write1Sec.ftni"
      INCLUDE "Accel2Vel.ftni"
      INCLUDE "LinFilt.ftni"