swfnav.f

Go to the documentation of this file.

        subroutine swfnav(navqc,navctl,input,nframes,orbit,nlines,
     *          navblk,tilts,xnodel,nodet)
c
c  swfnav(navqc,navctl,input,nframes,orbit,nlines,navblk)
c
c  purpose: main routine of seawifs navigation processing subsystem
c
c  calling arguments:
c
c  name         Type    i/o     description
c  --------     ----    ---     -----------
c  navqc        struct   i      navigation quality control parameters
c  navctl       struct   i      controls for processing and sensor, 
c                               offset data
c  input        struct   i      input data structure containing
c                               spacecraft id and time, tilt, 
c                               sun, magnetometer and earth sensor data
c                               as well as the s/c computed attitude
c  nframes      i*4      i      number of data frames in input
c  orbit        struct   i      structure containing processed orbit data
c  nlines       i*4      o      number of lines in the navblk structure
c  navblk       struct   o      processed navigation block data
c                               for every scan line of data
c  tilts        struct   o      tilt states for this scene and tilt angle 
c                               for every scan line
c  xnodel       r*4      o      longitude of descending node(degrees)
c  nodet        i*4      o      time of node crossing in milliseconds of day
c
c  by: w. robinson, gsc, 19 mar 93
c
c  notes:  
c
c  modification history:
c
c  august, 13 1993:  reworked to accept input data structure passed from 
c    frank chen's Level 1A processing soiftware and orbit data structure
c    generated by navigation initialization routine.  Added calls to 
c    nav_tlm subroutine to fill tlm structure, and orb_interp subroutine
c    to interpolate input orbit data to scan line times.
c    Frederick S. Patt, GSC
c
c  June 24, 1994:  Added tilt structure to pass to calling routine.
c    Frederick S. Patt, GSC
c
c  July 13, 1994:  Added call to get_node and orbit node parameters to 
c    pass to calling routine
c    Frederick S. Patt, GSC
c
c  October 1, 2001: Initialize navblock.nflag from input.nflag, so information
c    about timing shift corrections can be recorded.
c    B. Franz, SAIC.
c
c  October 30, 2007: Add warning message if nav flags are set in input.
c    F. Patt, SAIC.
 
      implicit none
c
#include "tlm_str.fin"
#include "navctl_s.fin"
#include "navqc_s.fin"
#include "navblk_s.fin"
#include "input_s.fin"
#include "orbit_s.fin"
#include "tilt_s.fin"
c
      type(navctl_struct) :: navctl
      type(navqc_struct) :: navqc
      type(navblk_struct) :: navblk(maxlin)
      type(tlm_struct) :: tlm
      type(input_struct) :: input(maxlin)
      type(orbit_struct) :: orbit
      type(tilt_states) :: tilts
c
      real*4 pos(3,maxlin), vel(3,maxlin), attxfm(3,3,maxlin),
     1     att_ang(3,maxlin), tiltpr(maxlin), xnodel
      real*8 time(maxlin), timref(3), tnode
      integer*4 gaclac, ntim, iret, nlines, lun, ilin, i, j, nframes,
     1     attangfl(maxlin), tiltfl(maxlin), orbfl(maxlin), nodet,
     1     iframe, nper
      logical*1 failflag
c
c
c       start, open a temporary data file
c
      lun = 8
.gt.      if (navctl%lvdbug0) 
     *  open( unit = lun, file = 'swfnav.out', status = 'unknown' )
c
c       extract navigation telemetry from input structure
c
      call navtlm(input,nframes,navctl,gaclac,tlm,tilts)
 
c       initialize nav block flags from input flags
c
      nper = 1
.eq.      if (gaclac  1) nper = 5
      do ilin = 1, nper*nframes
         iframe = (ilin-1)/nper + 1
         do i = 1, 8
            navblk(ilin)%nflag(i) = input(iframe)%nflag(i)
         end do
.eq.         if (navblk(ilin)%nflag(1)1) failflag = .true.
      end do
      
      if (failflag) print *,' navfail flag set in input data'
c
c       pre-process time tags to line-by-line time and reference
c
         ntim = tlm%ntlm
c
      call proctim2(input,nframes,navqc,time,timref,nlines,iret)
.eq.      if( iret  0 ) then
c
c          write out the processed time data
c
         write(6,100)gaclac, ntim, nlines, timref
  100    format(' swfnav: gaclac, ntim, nlines =',/,
     1     3i7,//,'  reference time(year, day, sec)=',
     1     /,3f12.4,/)
.gt.         if (navctl%lvdbug0) then
           write(lun,100)gaclac, ntim, nlines, timref
           do ilin = 1, nlines
             write(lun,200)ilin,time(ilin)
c            write(6,200)ilin,time(ilin)
  200        format(2x,i7,f14.4)
           end do
         end if
 
c       interpolate orbit data to scan line times
c
         call orb_interp(orbit,nlines,timref,time,pos,vel,orbfl)
c
c       check for orbit interpolation error 
c
.ne..or..ne.         if ((orbfl(1)0)  (orbfl(nlines)0)) then
            do ilin = 1,nlines
               navblk(ilin)%nflag(1) = 1
               navblk(ilin)%nflag(2) = 1
            end do
            go to 998
         end if
 
c       find node crossing for scene
c
         call get_node(nlines,timref,time,pos,vel,xnodel,tnode)
         nodet = tnode*1000.
c
c          compute attitude transform matrix array for the segment
c
         call cmpaxm( nlines, pos, vel, attxfm )
c
.gt.c         if (navctl%lvdbug0) then
c           do ilin = 1,nlines
c            write(6,300)ilin,((attxfm(i,j,ilin),i=1,3),j=1,3)
c             write(lun,300)ilin,((attxfm(i,j,ilin),i=1,3),j=1,3)
c  300        format(' attitude xform matrix for line',i7,/,
c     1       3(3f12.6,/))
c           end do
c        end if
c
c          this branch will use the on-board attitude 
c
.eq.         if( navctl%procatt  0 )then
c
c             process S/C based pitch, roll, yaw to line-by-line data
c
            print *,' swfnav: using s/c provided attitude'
            call sc_att(gaclac, tlm, navqc, att_ang, attangfl, iret )
c
c             print out the fitted pitch, roll, yaw
c
.gt.            if (navctl%lvdbug0) then
c            write(6,500)
              write(lun,500)
  500         format(//,' fitted yaw, roll, pitch follows')
              do ilin = 1,nlines
c              write(6,400)attangfl(ilin),(att_ang(i,ilin),i=1,3)
              write(lun,400)attangfl(ilin),(att_ang(i,ilin),i=1,3)
  400           format(i7,3f12.7)
              end do
            end if
c
c       prepare the tilt values reported
c
            call tiltcomp( nlines, tlm, timref, time, gaclac, navqc, 
     1          tiltpr, tiltfl )
c
c             compute remaining nav block parameters
c
            call scpar(nlines, attxfm, att_ang, attangfl,
     1     timref, time, pos, tiltpr, tiltfl, navctl, navblk)
c
c             this branch will use sun and earth sensor data to compute attitude
c
         else
            print *,' swfnav: determining attitude from sensor data'
            call attcmp( nlines, gaclac, tlm, pos, vel, timref, 
     1           time, navctl, navqc, attxfm, navblk, tiltpr, tiltfl)
         end if
c
c          place any remaining data into the navigation block
c
         do ilin = 1,nlines
            tilts%tilt(ilin) = tiltpr(ilin)
            do j = 1,3
               navblk(ilin)%orb_vec(j) = pos(j,ilin)
            end do
         end do
c
      else
c
c       for bad time processing, fill correct flags in nav block
c
         do ilin = 1, nlines
            navblk(ilin)%nflag(1) = 1
            navblk(ilin)%nflag(6) = 1
         end do
      end if
c
.gt. 998  if (navctl%lvdbug0) close( unit = lun )
c
c       for debug - write out some of the navblk info
c
.gt.      if (navctl%lvdbug0) then
        open(unit=lun, file = 'navblk.out', status = 'unknown' )
        do ilin = 1, nlines
         write(lun,600)(navblk(ilin)%orb_vec(j),j=1,3),
     1     (navblk(ilin)%att_ang(j),j=1,3)
  600    format(2x,3f13.4,3f13.7)
        end do
        close(unit=lun)
 
        open(unit=lun, file = 'tilt.out', status = 'unknown' )
        do ilin = 1, nlines
          write (lun,*) ilin,tilts%tilt(ilin)
        end do
        close(unit=lun)
      end if
c
c       and return
c
      return
      end