orbit.f 13.4 KB
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438
      subroutine orbit (day,kode,lfile,nsat,x,revnum,ier)
c*
c***********************************************************************
c*
c*          "Copyright [c] CNES 98 - tous droits reserves"
c*          **********************************************
c*
c*PRO MAGLIB
c*
c*VER 99.03.31 - V 1.0
c*VER 01.06.05 - V 2.0
c*VER 03.01.06 - V 2.1
c*
c*AUT spec. ESA - janvier 1991
c*AUT port. CISI
c*
c*
c*ROL Theme : Astronomie et calcul d'orbite
c*ROL         Calcul du vecteur d'etat a partir des donnees 
c*ROL         d'orbitographie de Cluster (compsants de Chebychev)
c*ROL         par resolution de l'equation de Kepler.
c*ROL         Un fichier de coefficients de Chebychev est necessaire.
c*
c*PAR day    (I) : date julienne ref 2000 (a partir de 01/01/2000)
c*
c*PAR kode   (I) : dimension du vecteur d'etat
c*
c*PAR lfile  (I) : numero logique associe au fichier en entree
c*
c*PAR nsat   (O) : identification du satellite 1, 2, 3 ou 4
c*
c*PAR x      (O) : vecteur d'etat : position en x, y et z du satellite
c*               : (kilometres) ; vitesse en x, y et z (kilometres/secondes)
c*
c*PAR revnum (O) : numero d'orbite
c*
c*PAR ier    (O) : code de retour
c*
c*NOT kode       : 3 = position
c*NOT kode       : 6 = position et vitesse
c*
c*NOT ier        : 0 = OK 
c*NOT ier        : 1 = date julienne erronee (trop petite)
c*NOT ier        : 2 = date julienne erronee (trop grande)
c*NOT ier        : 3 = non continuite de la date entre 2 enreg consecutifs
c*NOT ier        : 4 = valeur de kode erronee (differente de 3 ou 6)
c*NOT ier        : 5 = fichier incoherent (donnees invalides)
c*NOT ier        : 6 = erreur de lecture     
c*
c*NOT            : Le fichier doit etre ouvert au prealable.
c*NOT            : Les enregistrements doivent avoir des dates consecutives
c*NOT            : (date debut enreg i - date de fin enreg i-1 > gap 
c*NOT            : avec gap = 2.d-4)
c*
c*NOT common     : ssave
c*NOT            : Common de sauvegarde des donnees remanentes.
c*
c*INF utilise    : sans objet
c*
c*HST version 1.0 - 99.03.31 - creation de la maglib au CDPP
c*HST version 2.0 - 01.06.05 - correction de commentaires de code
c*HST version 2.1 - 03.01.06 - corrections en compilation avec g77
c*
c***********************************************************************
c*
      implicit none
c
c     ---------------------------------
c*FON Declaration identificateur rcs_id
c     ---------------------------------
c
      character rcs_id*100
c
c     --------------------------
c*FON Declaration des parametres
c     --------------------------
c
      double precision day
      integer kode,lfile,nsat
      double precision x(kode),revnum
      integer ier 
c
c     ----------------------------------
c*FON Declaration des variables communes
c     ----------------------------------
c
c     indicateur de lecture pour le 1er appel 
      integer mfile
c     date debut et date fin de l'enreg courant
      double precision daybeg,dayend
c     date de debut du fichier - marge 
c     date de fin du bloc courant + marge
      double precision dayfir,daylas
c
      common/ssave/daybeg,dayend,dayfir,daylas,mfile
c
c     --------------------------------- 
c*FON Declaration des variables locales
c     --------------------------------- 
c
      integer nrec
      integer iter
      integer k,i,j,l
c*LOC k,i,j,l : indices de boucles 
c
      double precision epoch
c*LOC epoch : date de reference du vecteur d'etat
c
      double precision revepo
c*LOC revepo : numero d'orbite associe
c
      double precision smaxis
c*LOC smaxis : demi grand axe associe en km
c
      double precision omotin
c*LOC omotin : mouvement moyen inverse pour une orbite Keplerienne
c
      double precision coeff(10,6)
c*LOC coeff : table des coefficients du polynome de Chebyshev pour les
c*LOC         6 composants du vecteur d'etat
c
      integer koeff
c*LOC koeff : nombre de coefficients polynomiaux (0< koeff <10) 
c
      double precision y(6)
c*LOC y : vecteur d'etat
c
      double precision rdist
c*LOC rdist : distance centre terre satellite a la date courante en km
c
      double precision s
c*LOC s : coefficient poynomial de Chebyshev
c
      double precision dmanom
      double precision arin,arm,rvwam,tam,comp,b,go,g1,bet
      double precision d,g2,g3,fx,gx,rx,ft,gt
      double precision daymid
      double precision scale
      double precision pa,p,pb
c*LOC Variables de travail intermediaires
c
      SAVE
c
c     ---------------------------------
c*FON Affectation identificateur rcs_id
c     ---------------------------------
c
      data rcs_id /"
     >$Id$"/
c
c     ******************
c     Debut de programme
c     ******************
c
      ier = 0
c
      if (kode .le. 0) goto 902
      if (kode .gt. 6) goto 902
c
c     cas de la 1er lecture                   
      if (lfile .ne. mfile) goto 10
c
c    ********************************************************
c*FON Recherche du bloc contenant la date passee en parametre
c    ********************************************************
c
c     -----------------------------------------------------------------
c*FON Gestion de la lecture en fonction de la date du dernier bloc lu.
c*FON Lecture des blocs anterieurs si la date est < date debut du bloc.
c*FON Lecture des blocs suivants si la date est > date fin du bloc.
c*FON Pas de lecture dans les autres cas.
c    ------------------------------------------------------------------
c
      if (day .gt. dayend + 1.d-4) goto 20
      if (day .ge. daybeg - 1.d-4) goto 70
c
c     ------------------------------------------------------------------
c*FON Initialisation de la lecture et positionnement en debut de fichier
c     ------------------------------------------------------------------
c
10    continue
      dayfir = 99.d9
      daylas = 99.d9
      mfile  = lfile
      rewind lfile
c
c     -------------------------------------------------------------
c*FON Recherche du 1er enregistrement du bloc (numero du satellite)
c     -------------------------------------------------------------
c
20    continue
      read (lfile,1000,err=901,end=100) nsat
      if (nsat .le. 0) goto 20
      if (nsat .gt. 4) goto 20
c
c*    -----------------------------
c*FON Lecture du 2nd enregistrenent
c*    -----------------------------
c*FON  structure de l'enregistrement :
c*FON  nrec = identification de l'enreg (doit etre egal a 200 + nsat)
c*FON  daybeg = date debut de l'enreg (date julienne)
c*FON  dayend = date fin de l'enreg (date julienne)
c*FON  epoch =  epoque de reference du vecteur etat (date julienne)
c*FON  revepo = numero d'orbite
c*FON  smaxis = demi grand axe en km
c*FON  omotin = mouvement moyen inverse pour une orbite Keplerienne
c
      read (lfile,1100,err=901,end=100)
     >      nrec,daybeg,dayend,epoch,revepo,smaxis,omotin
c
c     ----------------------------------
c*FON Verification du 2nd enregistrement
c     ----------------------------------
c
      if (nrec .ne. 200 + nsat) goto 900
      if (daybeg .gt. dayend) goto 900
c
c     ------------------------------------------------------------------
c*FON Controle de la date et de la continuite des enregistrements
c*FON La date doit etre > la + petite date deja lu - marge
c*FON L'ecart entre la date fin de l'enreg precedent et la date debut de 
c*FON l'enreg courant doit etre < gap
c*FON marge = 1.d-4 et gap = 2.d-4
c     ------------------------------------------------------------------
c
      dayfir = dmin1(dayfir,daybeg - 1.d-4)
      if (day .lt. dayfir) goto 905
      if (daybeg .gt. daylas) goto 903
c
      daylas = dayend + 2.d-4
c
c     ------------------------------------------------------------------
c*FON Gestion de la lecture en fonction de la date du bloc :
c*FON date  <  date debut du bloc - marge : lecture des blocs anterieurs
c*FON date  >  date fin du bloc + marge   : lecture des blocs suivants
c     ------------------------------------------------------------------
c
      if (day .gt. dayend + 1.d-4) goto 20
      if (day .lt. daybeg - 1.d-4) goto 10
c
c     -----------------------------
c*FON Lecture du 3ieme enregistrenent
c*    -----------------------------
c*FON  structure de l'enregistrement :
c*FON  nrec = id de l`enreg (doit etre egal a 300 + nb de coeff polynomiaux)
c*FON  y(6) = vecteur d'etat pour l'orbite de Kepler (km, km/s)
c*FON  rdist = distance centre terre satellite a la date courante en km
c
      read (lfile,1200,err=901,end=900) nrec,y,rdist
c
c     ------------------------------------
c*FON verification du 3ieme enregistrement
c     ------------------------------------
c
      if (nrec .gt. 310) goto 900
      if (nrec .lt. 300) goto 900
      koeff = nrec - 300
c
      if (koeff. le. 0) goto 70
c
c     ----------------------------------------------------------
c*FON Lecture des coefficients du polynome de Chebyshev pour les
c*FON 6 composants du vecteur d'etat
c*    ----------------------------------------------------------
c
      do 60 k = 1, koeff
         read (lfile,1300,err=901,end=900) nrec,(coeff(k,i),i=1,6)
c
c        ------------------------------------------------------
c*FON    Verification de l'enregistrement
c*FON    nrec = id de l`enreg (doit etre egal a koeff + 11 * k)  
c*       ------------------------------------------------------
c
         if (11 * k + koeff .ne. nrec) goto 900
60    continue
c
c*     -----------------
c*FON  Fin de la lecture
c*     -----------------
c
70    continue
c   
c     *****************************************
c*FON Debut de resolution de l'orbite de Kepler
c     *****************************************
c
c     -------------------------------------------------------
c*FON Convertion de la difference de date en anomalie moyenne
c*FON Calcul du numero d'orbite
c     -------------------------------------------------------
c
      dmanom = (day - epoch) * 864.d2 / omotin
      revnum = revepo + dmanom / 6.2831853072d0
c
      arin = smaxis / rdist
      arm  = (rdist - smaxis) / smaxis
      rvwam = (y(1) * y(4) + y(2) * y(5) + y(3) * y(6)) * omotin
     >        / smaxis**2
c
c*    --------------------------------------------------------
c*FON Calcul de l'anomalie excentrique par iteration de Newton
c*    --------------------------------------------------------
c
      tam = dmanom - rvwam
      comp = 1.d-7 + 1.d-10 * dabs(tam)
      b = tam
c
c     ---------------------------------------------
c*FON Iterations pour resoudre l'equation de Kepler
c     ---------------------------------------------
c
      do 130 iter = 1, 15
         go  = cos(b)
         g1  = sin(b)
         bet = tam - arm * g1 + rvwam * go
         d   = (bet - b) / (1.d0 + arm * go + rvwam * g1)
         b   = b + d
c
c        -----------------
c*FON    Condition d'arret 
c        -----------------
c
         if (dabs(d) .le. comp) goto 140
130   continue
c
c     ------------------
c*FON Pas de convergence       
c     ------------------
c
      goto 900
140   continue
      go = go - d * g1
      g1 = g1 + d * go
      g2 = 1.d0 - go
      g3 = b - g1
      fx = 1.d0  - g2 * arin
      gx = (dmanom - g3) * omotin
c
      k = min0(kode,3)
      do 150 j = 1, k
         x(j) = fx * y(j) + gx * y(j+3)
150   continue
c
      if (kode .le. 3) goto 170
      rx = sqrt(x(1)**2 + x(2)**2 + x(3)**2)
      ft = -g1 * smaxis * arin / (omotin * rx)
      gt = 1.d0 - g2 * smaxis / rx
      do 160 j = 4, kode
         x(j) = ft * y(j-3) + gt * y(j)
160   continue
c
c*    ******************************************
c*FON Fin de la resolution de l'orbite de Kepler
c*    ******************************************
c
170   continue
c
c     ---------------------------------------------------------
c*FON Verification des coeff polynomiaux (1 n'est pas autorise)
c     ---------------------------------------------------------
c
      if (koeff .le. 1) goto 600
c
c     ----------------------------------------------------------------------
c*FON Date milieu et vecteur d'echelle pour le coeff polynomial de Chebyshev
c     ----------------------------------------------------------------------
c
      daymid = 0.5d0 * (daybeg + dayend)
      scale  = 4.d0 / (dayend - daybeg)
      s      = scale * (day - daymid)
      pa     = 1.d0
      p      = s * 0.5d0
c
c     --------------------------------
c*FON Initialisation du vecteur d'etat 
c     --------------------------------
c
      do 200 j = 1, kode
         x(j) = x(j) + coeff(1,j) + coeff(2,j) * p
200   continue
c
      if (koeff .le. 2) goto 600
      do 210 l = 3, koeff
         pb = pa
         pa = p
         p  = s * pa - pb
         do 220 j = 1, kode
            x(j) = x(j) + coeff(l,j)*p
220      continue
210   continue
c
      goto 600
c
c     -------------------
c*FON Gestion des erreurs
c     -------------------
c
100   continue
c
c     ---------------------------------------------------
c*FON Cas de la fin de fichier : 
c*FON Cas de date trop grande si le 1er enreg est deja lu 
c*FON Donnees invalide dans les autres cas
c     ---------------------------------------------------
c
      if (daylas .lt. 1.d9) goto 904
c
900   continue
      ier = -1
901   continue
      ier = ier + 2
902   continue
      ier = ier + 1
903   continue
      ier = ier + 1
904   continue
      ier = ier + 1
905   continue
      ier = ier + 1
c
c     -----------------------------------------------------------
c*FON Re_initialisation de mfile pour forcer une lecture conplete
c*FON au prochain appel
c     -----------------------------------------------------------
c
      mfile = -9999
600   continue
c
c     ****************
c     Fin de programme
c     ****************
c
      return
1000  format(i3)
1100  format(i3,2f12.6,f15.9,f11.3,2f13.5)
1200  format(i3,3f11.3,3f11.7,f11.3)
1300  format(i3,3f11.3,3f11.7)
      end