sunwgt.f

Go to the documentation of this file.

        subroutine sunwgt(sun_bod,procrng,sunproc,nlines,wsun,wsunfl)
c
c  sunwgt(sun_bod,procrng,sunproc,nlines,wsun,wsunfl)
c
c  purpose: blend or weight the sun vector from the 3 sensors
c           into one consistent set
c
c  calling arguments:
c
c  name         Type    i/o     description
c  --------     ----    ---     -----------
c  sun_bod      r*4      i      size 3 by 3 sensor by nlines sun vector 
c                               data 
c  procrng      i*4      i      size 2 by 3 sensors low and high active 
c                               ranges for each sensor
c  sunproc      struct   i      contains flags for the sensor and line,
c                               0 = good, 1 = bad
c  nlines       i*4      i      number of lines of data
c  wsun         r*4      o      size 3 by nlines weighted sun vector data
c  wsunfl       i*4      o      size nlines flags for the weighted sun vectors
c
c  by: w. robinson, gsc, 6 apr 93
c
c  notes:  
c
c  modification history:
c
c  march 30, 1995:  modified weighting scheme to account for sun sensor
c  data as tangents of angles and to allow for possible swapping of axes.  
c  frederick s. patt, gsc
c
c  october 16, 1997.  modified the maximum sensor value to correspond to 
c  a restricted range of useful sensor data (tangent value of +/- 1.9).
c  frederick s. patt, gsc
 
      implicit none
c
#include "nav_cnst.fin"
#include "sunpr_s.fin"
c
      type(sunproc_struct) :: sunproc(3)
c
      integer*4 procrng(2,3), nlines, wsunfl(nlines)
      real*4 sun_bod(3,3,nlines), wsun(3,nlines), sumwgt
c
      integer*4 ivec, ilin, isens
      real*4 weight,sunmax
      data sunmax/1.9/
c
c
c       start, loop through the lines
c
      do ilin = 1, nlines
c
c         initialize weight, final sun vectors and set final flag to bad
c
        sumwgt = 0
        wsunfl(ilin) = 0
        do ivec = 1, 3
          wsun(ivec,ilin) = 0
        end do
c
c         loop over each sensor
c
        do isens = 1, 3
c
c             compute the weight for this sensor and line as 
c             the position from the end of the sensor range
 
            if( sunproc(isens)%flag(ilin) .eq. 0 ) then
 
            weight = sunmax - sqrt(sunproc(isens)%ang(1,ilin)**2 +
     *           sunproc(isens)%ang(2,ilin)**2)
c
c             get the sum of the weights, and weight * vector
c
            sumwgt = sumwgt + weight
            do ivec = 1, 3
              wsun(ivec,ilin) = wsun(ivec,ilin) + 
     1               sun_bod(ivec,isens,ilin) * weight
            end do
            wsunfl(ilin) = 0
c
          end if
        end do
c
c         divide the sum by the weight
c
        if( sumwgt .gt. 0 ) then
          do ivec = 1, 3
            wsun(ivec,ilin) = wsun(ivec,ilin) / sumwgt
          end do
        else
          wsunfl(ilin) = 1
        end if
c
      end do
c
      return
      end