SUBROUTINE Calc_Vz

c Subroutine to calculate aircraft-referenced vertical speed
c   from each 40 Hz sample and store in common array
c   All values are in system common, so no passed params

      IMPLICIT none

      INCLUDE "FastCom.ftni"


C Local storage
      INTEGER i
      REAL Hd,Pc,Rl

      DO i = 1,40  ! do all samples this sec

        Hd = Hdg(i)  ! easier to work with below
        Pc = Pitch(i) ! all are already in radians
        Rl = Roll(i)


C Only do the calc if attitude info is grossly reasonable
        IF (Hd.GE.-3.1416 .AND. Hd.LE.6.2832 .AND.  ! -180/+360 deg
     &      Pc.GE.-.785 .AND. Pc.LE.0.785 .AND.     ! -/+ 45 deg
     &      Rl.GE.-1.57 .AND. Rl.LE.1.57) THEN      ! -/+90 deg

          Vz(i) =
     &      Sin(Rl)*Cos(Hd)*EVel(i) -
     &      Cos(Rl)*Sin(Pc)*Sin(hd)*EVel(i) -
     &      Sin(Rl)*Sin(Hd)*NVel(i) -
     &      Cos(Rl)*Sin(Pc)*Cos(Hd)*NVel(i) +
     &      Cos(Rl)*Cos(Pc)*VVel(i)
        ELSE  ! bad angle
          Vz(i) = -999.  ! flag it for Nz calc
        ENDIF

      END DO  ! end of loop on 40 samples

      RETURN

      END