BrcKeplerOrbit.cpp
Go to the documentation of this file.
1 //==============================================================================
2 //
3 // This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
4 //
5 // The GNSSTk is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published
7 // by the Free Software Foundation; either version 3.0 of the License, or
8 // any later version.
9 //
10 // The GNSSTk is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public
16 // License along with GNSSTk; if not, write to the Free Software Foundation,
17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
18 //
19 // This software was developed by Applied Research Laboratories at the
20 // University of Texas at Austin.
21 // Copyright 2004-2022, The Board of Regents of The University of Texas System
22 //
23 //==============================================================================
24 
25 //==============================================================================
26 //
27 // This software was developed by Applied Research Laboratories at the
28 // University of Texas at Austin, under contract to an agency or agencies
29 // within the U.S. Department of Defense. The U.S. Government retains all
30 // rights to use, duplicate, distribute, disclose, or release this software.
31 //
32 // Pursuant to DoD Directive 523024
33 //
34 // DISTRIBUTION STATEMENT A: This software has been approved for public
35 // release, distribution is unlimited.
36 //
37 //==============================================================================
38 
43 #include <stdio.h>
44 #include "BrcKeplerOrbit.hpp"
45 #include <cmath>
46 
47 namespace gnsstk
48 {
49  using namespace std;
50  using namespace gnsstk;
51 
53  noexcept
54  {
55  dataLoaded = false;
56 
57  PRNID = 0;
58 
59  satSys = "";
60 
61  healthy = false;
62 
63  URAoe = 0;
64 
65  Cuc = Cus = Crc = Crs = Cic = Cis = M0 = dn = dndot =
66  ecc = A = Ahalf =Adot = OMEGA0 = i0 = w = OMEGAdot = idot = 0.0;
67  }
68 
69  BrcKeplerOrbit::BrcKeplerOrbit(const std::string satSysArg,
70  const ObsID obsIDArg,
71  const short PRNIDArg,
72  const CommonTime beginFitArg,
73  const CommonTime endFitArg,
74  const CommonTime ToeArg,
75  const short URAoeArg, const bool healthyArg,
76  const double CucArg, const double CusArg,
77  const double CrcArg, const double CrsArg,
78  const double CicArg, const double CisArg,
79  const double M0Arg, const double dnArg,
80  const double dndotArg, const double eccArg,
81  const double AArg, const double AhalfArg,
82  const double AdotArg, const double OMEGA0Arg,
83  const double i0Arg, const double wArg,
84  const double OMEGAdotARg,
85  const double idotArg )
86  {
87  loadData(satSysArg, obsIDArg, PRNIDArg, beginFitArg, endFitArg, ToeArg,
88  URAoeArg, healthyArg, CucArg, CusArg, CrcArg,
89  CrsArg, CicArg, CisArg, M0Arg, dnArg, dndotArg, eccArg, AArg,
90  AhalfArg, AdotArg, OMEGA0Arg, i0Arg, wArg, OMEGAdotARg, idotArg);
91  }
92 
94  BrcKeplerOrbit::BrcKeplerOrbit(const ObsID obsIDArg, const short PRNID,
95  const short fullweeknum,
96  const long subframe1[10],
97  const long subframe2[10],
98  const long subframe3[10] )
99  {
100  loadData(obsIDArg, PRNID,fullweeknum, subframe1, subframe2, subframe3 );
101  }
102 
103 
104  bool BrcKeplerOrbit::operator==(const BrcKeplerOrbit& right) const noexcept
105  {
106  return ((dataLoaded == right.dataLoaded) &&
107  (satSys == right.satSys) &&
108  (obsID == right.obsID) &&
109  (PRNID == right.PRNID) &&
110  (Toe == right.Toe) &&
111  (URAoe == right.URAoe) &&
112  (healthy == right.healthy) &&
113  (Cuc == right.Cuc) &&
114  (Cus == right.Cus) &&
115  (Crc == right.Crc) &&
116  (Crs == right.Crs) &&
117  (Cic == right.Cic) &&
118  (Cis == right.Cis) &&
119  (M0 == right.M0) &&
120  (dn == right.dn) &&
121  (dndot == right.dndot) &&
122  (ecc == right.ecc) &&
123  (A == right.A) &&
124  (Ahalf == right.Ahalf) &&
125  (Adot == right.Adot) &&
126  (OMEGA0 == right.OMEGA0) &&
127  (i0 == right.i0) &&
128  (w == right.w) &&
129  (OMEGAdot == right.OMEGAdot) &&
130  (idot == right.idot) &&
131  (beginFit == right.beginFit) &&
132  (endFit == right.endFit));
133  }
134 
135 
136  void BrcKeplerOrbit::loadData(const std::string satSysArg,
137  const ObsID obsIDArg,
138  const short PRNIDArg,
139  const CommonTime beginFitArg,
140  const CommonTime endFitArg,
141  const CommonTime ToeArg,
142  const short URAoeArg, const bool healthyArg,
143  const double CucArg, const double CusArg,
144  const double CrcArg, const double CrsArg,
145  const double CicArg, const double CisArg,
146  const double M0Arg, const double dnArg,
147  const double dndotArg, const double eccArg,
148  const double AArg, const double AhalfArg,
149  const double AdotArg, const double OMEGA0Arg,
150  const double i0Arg, const double wArg,
151  const double OMEGAdotARg, const double idotArg)
152  {
153  satSys = satSysArg;
154  obsID = obsIDArg;
155  PRNID = PRNIDArg;
156  beginFit = beginFitArg;
157  endFit = endFitArg;
158  Toe = ToeArg;
159  URAoe = URAoeArg;
160  healthy = healthyArg;
161  Cuc = CucArg;
162  Cus = CusArg;
163  Crc = CrcArg;
164  Crs = CrsArg;
165  Cic = CicArg;
166  Cis = CisArg;
167  M0 = M0Arg;
168  dn = dnArg;
169  dndot = dndotArg;
170  ecc = eccArg;
171  A = AArg;
172  Ahalf = AhalfArg;
173  Adot = AdotArg;
174  OMEGA0 = OMEGA0Arg;
175  i0 = i0Arg;
176  w = wArg;
177  OMEGAdot = OMEGAdotARg;
178  idot = idotArg;
179  dataLoaded = true;
180  }
181 
182  void BrcKeplerOrbit::loadData(const ObsID obsIDArg, const short PRNIDArg,
183  const short fullweeknum,
184  const long subframe1[10],
185  const long subframe2[10],
186  const long subframe3[10])
187  {
188  double ficked[60];
189 
190  //Load overhead members
191  satSys = "G";
192  obsID = obsIDArg;
193  PRNID = PRNIDArg;
194  short iodc = 0;
195 
196  //Convert Subframe 1
197  if (!subframeConvert(subframe1, fullweeknum, ficked))
198  {
199  InvalidParameter exc("Subframe 1 not valid.");
200  GNSSTK_THROW(exc);
201  }
202 
203  short weeknum = static_cast<short>( ficked[5] );
204  short accFlag = static_cast<short>( ficked[7] );
205  short health = static_cast<short>( ficked[8] );
206  URAoe = accFlag;
207  healthy = false;
208  if (health == 0)
209  healthy = true;
210  iodc = static_cast<short>( ldexp( ficked[9], -11 ) );
211 
212  //Convert Subframe 2
213  if (!subframeConvert(subframe2, fullweeknum, ficked))
214  {
215  InvalidParameter exc("Subframe 2 not valid.");
216  GNSSTK_THROW(exc);
217  }
218 
219  Crs = ficked[6];
220  dn = ficked[7];
221  M0 = ficked[8];
222  Cuc = ficked[9];
223  ecc = ficked[10];
224  Cus = ficked[11];
225  Ahalf = ficked[12];
226  A = Ahalf*Ahalf;
227  double ToeSOW = ficked[13];
228 /*
229  double diff = Txmit - ToeSOW;
230  if (diff > HALFWEEK) // NOTE: This USED to be in DayTime, but DayTime is going away. Where is it now?
231  weeknum++; // Convert week # of transmission to week # of epoch time when Toc is forward across a week boundary
232  else if (diff < -HALFWEEK)
233  weeknum--; // Convert week # of transmission to week # of epoch time when Toc is back across a week boundary
234 */
235  Toe = GPSWeekSecond(weeknum, ToeSOW, TimeSystem::GPS);
236  short fiti = static_cast<short>(ficked[14]);
237  short fitHours = getLegacyFitInterval(iodc, fiti);
238  long beginFitSOW = ToeSOW - (fitHours/2)*3600;
239  long endFitSOW = ToeSOW + (fitHours/2)*3600;
240  short beginFitWk = weeknum;
241  short endFitWk = weeknum;
242  if (beginFitSOW < 0)
243  {
244  beginFitSOW += FULLWEEK;
245  beginFitWk--;
246  }
247  beginFit = GPSWeekSecond(beginFitWk, beginFitSOW, TimeSystem::GPS);
248 
249  if (endFitSOW >= FULLWEEK)
250  {
251  endFitSOW -= FULLWEEK;
252  endFitWk++;
253  }
254  endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
255 
256  //Convert Subframe 3
257  if (!subframeConvert(subframe3, fullweeknum, ficked))
258  {
259  InvalidParameter exc("Subframe3 not valid.");
260  GNSSTK_THROW(exc);
261  }
262 
263  Cic = ficked[5];
264  OMEGA0 = ficked[6];
265  Cis = ficked[7];
266  i0 = ficked[8];
267  Crc = ficked[9];
268  w = ficked[10];
269  OMEGAdot = ficked[11];
270  idot = ficked[13];
271 
272  dndot = 0.0;
273  Adot = 0.0;
274  dataLoaded = true;
275 
276  return;
277  }
278 
280  {
281  return(dataLoaded);
282  }
283 
285  {
286  if (!dataLoaded)
287  {
288  InvalidRequest exc("Required data not stored.");
289  GNSSTK_THROW(exc);
290  }
291  return(healthy);
292  }
293 
295  {
296  if (!dataLoaded)
297  {
298  InvalidRequest exc("Required data not stored.");
299  GNSSTK_THROW(exc);
300  }
301  if (ct >= beginFit && ct <= endFit) return(true);
302  return(false);
303  }
304 
306  {
307  Xvt sv;
308 
309  GPSWeekSecond gpsws = (Toe);
310  double ToeSOW = gpsws.sow;
311  double ea; // eccentric anomaly //
312  double delea; // delta eccentric anomaly during iteration */
313  double elapte; // elapsed time since Toe
314  //double elaptc; // elapsed time since Toc
315  double q,sinea,cosea;
316  double GSTA,GCTA;
317  double amm;
318  double meana; // mean anomaly
319  double F,G; // temporary real variables
320  double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC;
321  double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc;
322  double xef,yef,zef,dek,dlk,div,domk,duv,drv;
323  double dxp,dyp,vxef,vyef,vzef;
324  GPSEllipsoid ell;
325 
326  double sqrtgm = SQRT(ell.gm());
327 
328  // Check for ground transmitter
329  double twoPI = 2.0e0 * PI;
330  double lecc; // eccentricity
331  double tdrinc; // dt inclination
332 
333  lecc = ecc;
334  tdrinc = idot;
335 
336  // Compute time since ephemeris & clock epochs
337  elapte = t - getOrbitEpoch();
338  //CommonTime orbEp = getOrbitEpoch();
339  //elapte = t - orbEp;
340 
341  // Compute mean motion
342  amm = (sqrtgm / (A*Ahalf)) + dn;
343 
344 
345  // In-plane angles
346  // meana - Mean anomaly
347  // ea - Eccentric anomaly
348  // truea - True anomaly
349 
350  meana = M0 + elapte * amm;
351  meana = fmod(meana, twoPI);
352 
353  ea = meana + lecc * ::sin(meana);
354 
355  int loop_cnt = 1;
356  do {
357  F = meana - ( ea - lecc * ::sin(ea));
358  G = 1.0 - lecc * ::cos(ea);
359  delea = F/G;
360  ea = ea + delea;
361  loop_cnt++;
362  } while ( (fabs(delea) > 1.0e-11 ) && (loop_cnt <= 20) );
363 
364  // Compute clock corrections
365  sv.relcorr = svRelativity(t);
366  // This appears to be only a string for naming
368 
369  // Compute true anomaly
370  q = SQRT( 1.0e0 - lecc*lecc);
371  sinea = ::sin(ea);
372  cosea = ::cos(ea);
373  G = 1.0e0 - lecc * cosea;
374 
375  // G*SIN(TA) AND G*COS(TA)
376  GSTA = q * sinea;
377  GCTA = cosea - lecc;
378 
379  // True anomaly
380  truea = atan2 ( GSTA, GCTA );
381 
382  // Argument of lat and correction terms (2nd harmonic)
383  alat = truea + w;
384  talat = 2.0e0 * alat;
385  c2al = ::cos( talat );
386  s2al = ::sin( talat );
387 
388  du = c2al * Cuc + s2al * Cus;
389  dr = c2al * Crc + s2al * Crs;
390  di = c2al * Cic + s2al * Cis;
391 
392  // U = updated argument of lat, R = radius, AINC = inclination
393  U = alat + du;
394  R = A*G + dr;
395  AINC = i0 + tdrinc * elapte + di;
396 
397  // Longitude of ascending node (ANLON)
398  ANLON = OMEGA0 + (OMEGAdot - ell.angVelocity()) *
399  elapte - ell.angVelocity() * ToeSOW;
400 
401  // In plane location
402  cosu = ::cos( U );
403  sinu = ::sin( U );
404 
405  xip = R * cosu;
406  yip = R * sinu;
407 
408  // Angles for rotation to earth fixed
409  can = ::cos( ANLON );
410  san = ::sin( ANLON );
411  cinc = ::cos( AINC );
412  sinc = ::sin( AINC );
413 
414  // Earth fixed - meters
415  xef = xip*can - yip*cinc*san;
416  yef = xip*san + yip*cinc*can;
417  zef = yip*sinc;
418 
419  sv.x[0] = xef;
420  sv.x[1] = yef;
421  sv.x[2] = zef;
422 
423  // Compute velocity of rotation coordinates
424  dek = amm / G;
425  dlk = amm * q / (G*G);
426  div = tdrinc - 2.0e0 * dlk *
427  ( Cic * s2al - Cis * c2al );
428  domk = OMEGAdot - ell.angVelocity();
429  duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al) );
430  drv = A * lecc * dek * sinea - 2.e0 * dlk *
431  ( Crc * s2al - Crs * c2al ) + Adot * G;
432 
433  dxp = drv*cosu - R*sinu*duv;
434  dyp = drv*sinu + R*cosu*duv;
435 
436  // Calculate velocities
437  vxef = dxp*can - xip*san*domk - dyp*cinc*san
438  + yip*( sinc*san*div - cinc*can*domk);
439  vyef = dxp*san + xip*can*domk + dyp*cinc*can
440  - yip*( sinc*can*div + cinc*san*domk);
441  vzef = dyp*sinc + yip*cinc*div;
442 
443  // Move results into output variables
444  sv.v[0] = vxef;
445  sv.v[1] = vyef;
446  sv.v[2] = vzef;
447  sv.health = healthy ? Xvt::Healthy : Xvt::Unhealthy;
448 
449  return sv;
450  }
451 
453  {
454  GPSEllipsoid ell;
455  double twoPI = 2.0e0 * PI;
456  double sqrtgm = SQRT(ell.gm());
457  double elapte = t - getOrbitEpoch();
458  double amm = (sqrtgm / (A*Ahalf)) + dn;
459  double meana,F,G,delea;
460 
461  meana = M0 + elapte * amm;
462  meana = fmod(meana, twoPI);
463  double ea = meana + ecc * ::sin(meana);
464 
465  int loop_cnt = 1;
466  do {
467  F = meana - ( ea - ecc * ::sin(ea));
468  G = 1.0 - ecc * ::cos(ea);
469  delea = F/G;
470  ea = ea + delea;
471  loop_cnt++;
472  } while ( (ABS(delea) > 1.0e-11 ) && (loop_cnt <= 20) );
473  double dtr = REL_CONST * ecc * Ahalf * ::sin(ea);
474  return dtr;
475  }
476 
478  {
479  return Toe;
480  }
481 
483  {
484  if (!dataLoaded)
485  {
486  InvalidRequest exc("Required data not stored.");
487  GNSSTK_THROW(exc);
488  }
489  return beginFit;
490  }
491 
493  {
494  if (!dataLoaded)
495  {
496  InvalidRequest exc("Required data not stored.");
497  GNSSTK_THROW(exc);
498  }
499  return endFit;
500  }
501 
503  {
504  if(!dataLoaded)
505  {
506  InvalidRequest exc("Required data not stored.");
507  GNSSTK_THROW(exc);
508  }
509  return PRNID;
510  }
511 
513  {
514  if(!dataLoaded)
515  {
516  InvalidRequest exc("Required data not stored.");
517  GNSSTK_THROW(exc);
518  }
519  return obsID;
520  }
521 
523  {
524  if (!dataLoaded)
525  {
526  InvalidRequest exc("Required data not stored.");
527  GNSSTK_THROW(exc);
528  }
529  GPSWeekSecond gpsws(Toe);
530  return (gpsws.week);
531  }
532 
534  {
535  if (!dataLoaded)
536  {
537  InvalidRequest exc("Required data not stored.");
538  GNSSTK_THROW(exc);
539  }
540  double accuracy = ura2CNAVaccuracy(URAoe);
541  return accuracy;
542  }
543 
544  void BrcKeplerOrbit::setAccuracy(const double& acc)
545  {
546  if (!dataLoaded)
547  {
548  InvalidRequest exc("Required data not stored.");
549  GNSSTK_THROW(exc);
550  }
551  URAoe = accuracy2ura(acc);
552  }
553 
555  {
556  if (!dataLoaded)
557  {
558  InvalidRequest exc("Required data not stored.");
559  GNSSTK_THROW(exc);
560  }
561  return URAoe;
562  }
563 
564  double BrcKeplerOrbit::getCus() const
565  {
566  if (!dataLoaded)
567  {
568  InvalidRequest exc("Required data not stored.");
569  GNSSTK_THROW(exc);
570  }
571  return Cus;
572  }
573 
574  double BrcKeplerOrbit::getCrs() const
575  {
576  if (!dataLoaded)
577  {
578  InvalidRequest exc("Required data not stored.");
579  GNSSTK_THROW(exc);
580  }
581  return Crs;
582  }
583 
584  double BrcKeplerOrbit::getCis() const
585  {
586  if (!dataLoaded)
587  {
588  InvalidRequest exc("Required data not stored.");
589  GNSSTK_THROW(exc);
590  }
591  return Cis;
592  }
593 
594  double BrcKeplerOrbit::getCrc() const
595  {
596  if (!dataLoaded)
597  {
598  InvalidRequest exc("Required data not stored.");
599  GNSSTK_THROW(exc);
600  }
601  return Crc;
602  }
603 
604  double BrcKeplerOrbit::getCuc() const
605  {
606  if (!dataLoaded)
607  {
608  InvalidRequest exc("Required data not stored.");
609  GNSSTK_THROW(exc);
610  }
611  return Cuc;
612  }
613 
614  double BrcKeplerOrbit::getCic() const
615  {
616  if (!dataLoaded)
617  {
618  InvalidRequest exc("Required data not stored.");
619  GNSSTK_THROW(exc);
620  }
621  return Cic;
622  }
623 
624  double BrcKeplerOrbit::getToe() const
625  {
626  if (!dataLoaded)
627  {
628  InvalidRequest exc("Required data not stored.");
629  GNSSTK_THROW(exc);
630  }
631  GPSWeekSecond gpsws(Toe);
632  return gpsws.sow;
633  }
634 
635  double BrcKeplerOrbit::getM0() const
636  {
637  if (!dataLoaded)
638  {
639  InvalidRequest exc("Required data not stored.");
640  GNSSTK_THROW(exc);
641  }
642  return M0;
643  }
644 
645  double BrcKeplerOrbit::getDn() const
646  {
647  if (!dataLoaded)
648  {
649  InvalidRequest exc("Required data not stored.");
650  GNSSTK_THROW(exc);
651  }
652  return dn;
653  }
654 
655  double BrcKeplerOrbit::getEcc() const
656  {
657  if (!dataLoaded)
658  {
659  InvalidRequest exc("Required data not stored.");
660  GNSSTK_THROW(exc);
661  }
662  return ecc;
663  }
664 
665  double BrcKeplerOrbit::getA() const
666  {
667  if (!dataLoaded)
668  {
669  InvalidRequest exc("Required data not stored.");
670  GNSSTK_THROW(exc);
671  }
672  return A;
673  }
674 
676  {
677  if (!dataLoaded)
678  {
679  InvalidRequest exc("Required data not stored.");
680  GNSSTK_THROW(exc);
681  }
682  return Ahalf;
683  }
684 
685  double BrcKeplerOrbit::getAdot() const
686  {
687  if (!dataLoaded)
688  {
689  InvalidRequest exc("Required data not stored.");
690  GNSSTK_THROW(exc);
691  }
692  return Adot;
693  }
694 
696  {
697  if (!dataLoaded)
698  {
699  InvalidRequest exc("Required data not stored.");
700  GNSSTK_THROW(exc);
701  }
702  return dndot;
703  }
704 
706  {
707  if (!dataLoaded)
708  {
709  InvalidRequest exc("Required data not stored.");
710  GNSSTK_THROW(exc);
711  }
712  return OMEGA0;
713  }
714 
715  double BrcKeplerOrbit::getI0() const
716  {
717  if (!dataLoaded)
718  {
719  InvalidRequest exc("Required data not stored.");
720  GNSSTK_THROW(exc);
721  }
722  return i0;
723  }
724 
725  double BrcKeplerOrbit::getW() const
726  {
727  if (!dataLoaded)
728  {
729  InvalidRequest exc("Required data not stored.");
730  GNSSTK_THROW(exc);
731  }
732  return w;
733  }
734 
736  {
737  if (!dataLoaded)
738  {
739  InvalidRequest exc("Required data not stored.");
740  GNSSTK_THROW(exc);
741  }
742  return OMEGAdot;
743  }
744 
745  double BrcKeplerOrbit::getIDot() const
746  {
747  if (!dataLoaded)
748  {
749  InvalidRequest exc("Required data not stored.");
750  GNSSTK_THROW(exc);
751  }
752  return idot;
753  }
754 
755  static void timeDisplay( ostream & os, const CommonTime& t )
756  {
757  // Convert to CommonTime struct from GPS wk,SOW to M/D/Y, H:M:S.
758  GPSWeekSecond dummyTime;
759  dummyTime = GPSWeekSecond(t);
760  os << setw(4) << dummyTime.week << "(";
761  os << setw(4) << (dummyTime.week & 0x03FF) << ") ";
762  os << setw(6) << setfill(' ') << dummyTime.sow << " ";
763 
764  switch (dummyTime.getDayOfWeek())
765  {
766  case 0: os << "Sun-0"; break;
767  case 1: os << "Mon-1"; break;
768  case 2: os << "Tue-2"; break;
769  case 3: os << "Wed-3"; break;
770  case 4: os << "Thu-4"; break;
771  case 5: os << "Fri-5"; break;
772  case 6: os << "Sat-6"; break;
773  default: break;
774  }
775  os << " " << (static_cast<YDSTime>(t)).printf("%3j %5.0s ")
776  << (static_cast<CivilTime>(t)).printf("%02m/%02d/%04Y %02H:%02M:%02S");
777  }
778 #pragma clang diagnostic push
779 #pragma clang diagnostic ignored "-Wunused-function"
780  static void shortcut(ostream & os, const long HOW )
781  {
782  short DOW, hour, min, sec;
783  long SOD, SOW;
784  short SOH;
785 
786  SOW = static_cast<long>( HOW );
787  DOW = static_cast<short>( SOW / SEC_PER_DAY );
788  SOD = SOW - static_cast<long>( DOW * SEC_PER_DAY );
789  hour = static_cast<short>( SOD/3600 );
790 
791  SOH = static_cast<short>( SOD - (hour*3600) );
792  min = SOH/60;
793 
794  sec = SOH - min * 60;
795  switch (DOW)
796  {
797  case 0: os << "Sun-0"; break;
798  case 1: os << "Mon-1"; break;
799  case 2: os << "Tue-2"; break;
800  case 3: os << "Wed-3"; break;
801  case 4: os << "Thu-4"; break;
802  case 5: os << "Fri-5"; break;
803  case 6: os << "Sat-6"; break;
804  default: break;
805  }
806 
807  os << ":" << setfill('0')
808  << setw(2) << hour
809  << ":" << setw(2) << min
810  << ":" << setw(2) << sec
811  << setfill(' ');
812  }
813 #pragma clang diagnostic pop
814  void BrcKeplerOrbit::dump(ostream& s) const
815  noexcept
816  {
817  const ios::fmtflags oldFlags = s.flags();
818  s.setf(ios::fixed, ios::floatfield);
819  s.setf(ios::right, ios::adjustfield);
820  s.setf(ios::uppercase);
821  s.precision(0);
822  s.fill(' ');
823 
824  s << "****************************************************************"
825  << "************" << endl
826  << "Broadcast Ephemeris (Engineering Units)" << endl
827  << endl
828  << "PRN : " << setw(2) << PRNID << endl
829  << endl;
830 
831  s << " Week(10bt) SOW DOW UTD SOD"
832  << " MM/DD/YYYY HH:MM:SS\n";
833 
834  s << endl;
835  s << "Eph Epoch: ";
836  timeDisplay(s, getOrbitEpoch());
837  s << endl;
838 
839  s.setf(ios::scientific, ios::floatfield);
840  s.precision(8);
841 
842  s << endl
843  << " ORBIT PARAMETERS"
844  << endl
845  << endl
846  << "Semi-major axis: " << setw(16) << Ahalf << " m**.5" << endl
847  << "Motion correction: " << setw(16) << dn << " rad/sec"
848  << endl
849  << "Eccentricity: " << setw(16) << ecc << endl
850  << "Arg of perigee: " << setw(16) << w << " rad" << endl
851  << "Mean anomaly at epoch: " << setw(16) << M0 << " rad" << endl
852  << "Right ascension: " << setw(16) << OMEGA0 << " rad "
853  << setw(16) << OMEGAdot << " rad/sec" << endl
854  << "Inclination: " << setw(16) << i0 << " rad "
855  << setw(16) << idot << " rad/sec" << endl;
856 
857  s << endl
858  << " HARMONIC CORRECTIONS"
859  << endl
860  << endl
861  << "Radial Sine: " << setw(16) << Crs << " m Cosine: "
862  << setw(16) << Crc << " m" << endl
863  << "Inclination Sine: " << setw(16) << Cis << " rad Cosine: "
864  << setw(16) << Cic << " rad" << endl
865  << "In-track Sine: " << setw(16) << Cus << " rad Cosine: "
866  << setw(16) << Cuc << " rad" << endl;
867 
868  s << endl;
869 
870  s.flags(oldFlags);
871  } // end of BrcKeplerOrbit::dump()
872 
873  ostream& operator<<(ostream& s, const BrcKeplerOrbit& eph)
874  {
875  eph.dump(s);
876  return s;
877 
878  } // end of operator<<
879 
880 } // namespace
gnsstk::BrcKeplerOrbit::getI0
double getI0() const
Definition: BrcKeplerOrbit.cpp:715
gnsstk::BrcKeplerOrbit::getW
double getW() const
Definition: BrcKeplerOrbit.cpp:725
gnsstk::BrcKeplerOrbit::getURAoe
short getURAoe() const
Definition: BrcKeplerOrbit.cpp:554
gnsstk::BrcKeplerOrbit::getOmega0
double getOmega0() const
Definition: BrcKeplerOrbit.cpp:705
gnsstk::BrcKeplerOrbit::getBeginningOfFitInterval
CommonTime getBeginningOfFitInterval() const
Definition: BrcKeplerOrbit.cpp:482
gnsstk::BrcKeplerOrbit
Definition: BrcKeplerOrbit.hpp:81
gnsstk::BrcKeplerOrbit::hasData
bool hasData() const
Definition: BrcKeplerOrbit.cpp:279
SQRT
#define SQRT(x)
Definition: MathBase.hpp:74
gnsstk::SEC_PER_DAY
const long SEC_PER_DAY
Seconds per day.
Definition: TimeConstants.hpp:63
gnsstk::shortcut
static void shortcut(ostream &os, const long HOW)
Definition: BrcClockCorrection.cpp:377
gnsstk::YDSTime
Definition: YDSTime.hpp:58
gnsstk::GPSEllipsoid::angVelocity
virtual double angVelocity() const noexcept
Definition: GPSEllipsoid.hpp:72
gnsstk::Xvt::Healthy
@ Healthy
Satellite is healthy, PVT valid.
Definition: Xvt.hpp:96
gnsstk::FULLWEEK
const long FULLWEEK
Seconds per whole week.
Definition: TimeConstants.hpp:60
gnsstk::BrcKeplerOrbit::getIDot
double getIDot() const
Definition: BrcKeplerOrbit.cpp:745
BrcKeplerOrbit.hpp
gnsstk::BrcKeplerOrbit::setAccuracy
void setAccuracy(const double &acc)
Definition: BrcKeplerOrbit.cpp:544
example6.hour
hour
Definition: example6.py:67
gnsstk::Xvt::frame
RefFrame frame
reference frame of this data
Definition: Xvt.hpp:156
gnsstk::GPSEllipsoid
Definition: GPSEllipsoid.hpp:67
gnsstk::PI
const double PI
GPS value of PI; also specified by GAL.
Definition: GNSSconstants.hpp:62
gnsstk::Xvt::v
Triple v
satellite velocity in ECEF Cartesian, meters/second
Definition: Xvt.hpp:152
gnsstk::BrcKeplerOrbit::BrcKeplerOrbit
BrcKeplerOrbit() noexcept
Definition: BrcKeplerOrbit.cpp:52
gnsstk::RefFrame
Definition: RefFrame.hpp:53
gnsstk::BrcKeplerOrbit::getObsID
ObsID getObsID() const
Definition: BrcKeplerOrbit.cpp:512
gnsstk
For Sinex::InputHistory.
Definition: BasicFramework.cpp:50
gnsstk::BrcKeplerOrbit::getM0
double getM0() const
Definition: BrcKeplerOrbit.cpp:635
gnsstk::BrcKeplerOrbit::svXvt
Xvt svXvt(const CommonTime &t) const
Definition: BrcKeplerOrbit.cpp:305
std::sin
double sin(gnsstk::Angle x)
Definition: Angle.hpp:144
gnsstk::Xvt::relcorr
double relcorr
relativity correction (standard 2R.V/c^2 term), seconds
Definition: Xvt.hpp:155
gnsstk::GPSWeekSecond
Definition: GPSWeekSecond.hpp:56
gnsstk::WeekSecond::getDayOfWeek
virtual unsigned int getDayOfWeek() const
Force this interface on this classes descendants.
Definition: WeekSecond.hpp:134
gnsstk::BrcKeplerOrbit::getAdot
double getAdot() const
Definition: BrcKeplerOrbit.cpp:685
gnsstk::BrcKeplerOrbit::getPRNID
short getPRNID() const
Definition: BrcKeplerOrbit.cpp:502
gnsstk::Xvt::x
Triple x
Sat position ECEF Cartesian (X,Y,Z) meters.
Definition: Xvt.hpp:151
gnsstk::REL_CONST
const double REL_CONST
relativity constant (sec/sqrt(m))
Definition: GNSSconstants.hpp:72
gnsstk::BrcKeplerOrbit::getDnDot
double getDnDot() const
Definition: BrcKeplerOrbit.cpp:695
gnsstk::BrcKeplerOrbit::getToe
double getToe() const
Definition: BrcKeplerOrbit.cpp:624
gnsstk::BrcKeplerOrbit::getOmegaDot
double getOmegaDot() const
Definition: BrcKeplerOrbit.cpp:735
gnsstk::ObsID
Definition: ObsID.hpp:82
gnsstk::operator<<
std::ostream & operator<<(std::ostream &s, const ObsEpoch &oe) noexcept
Definition: ObsEpochMap.cpp:54
gnsstk::BrcKeplerOrbit::dump
void dump(std::ostream &s=std::cout) const noexcept
Definition: BrcKeplerOrbit.cpp:814
gnsstk::BrcKeplerOrbit::getAhalf
double getAhalf() const
Definition: BrcKeplerOrbit.cpp:675
gnsstk::CommonTime
Definition: CommonTime.hpp:84
gnsstk::WeekSecond::sow
double sow
Definition: WeekSecond.hpp:155
gnsstk::BrcKeplerOrbit::operator==
bool operator==(const BrcKeplerOrbit &right) const noexcept
Definition: BrcKeplerOrbit.cpp:104
gnsstk::RefFrameSys::WGS84
@ WGS84
The reference frame used by GPS.
gnsstk::min
T min(const SparseMatrix< T > &SM)
Maximum element - return 0 if empty.
Definition: SparseMatrix.hpp:858
gnsstk::getLegacyFitInterval
short getLegacyFitInterval(const short iodc, const short fiti)
Definition: GNSSconstants.hpp:111
gnsstk::BrcKeplerOrbit::getCus
double getCus() const
Definition: BrcKeplerOrbit.cpp:564
gnsstk::Xvt
Definition: Xvt.hpp:60
std::cos
double cos(gnsstk::Angle x)
Definition: Angle.hpp:146
gnsstk::Week::week
int week
Full week number.
Definition: Week.hpp:267
gnsstk::BrcKeplerOrbit::withinFitInterval
bool withinFitInterval(const CommonTime) const
Definition: BrcKeplerOrbit.cpp:294
gnsstk::BrcKeplerOrbit::getCic
double getCic() const
Definition: BrcKeplerOrbit.cpp:614
gnsstk::BrcKeplerOrbit::getFullWeek
short getFullWeek() const
Definition: BrcKeplerOrbit.cpp:522
gnsstk::BrcKeplerOrbit::getAccuracy
double getAccuracy() const
Definition: BrcKeplerOrbit.cpp:533
ABS
#define ABS(x)
Definition: MathBase.hpp:73
gnsstk::BrcKeplerOrbit::getCrs
double getCrs() const
Definition: BrcKeplerOrbit.cpp:574
gnsstk::Xvt::health
HealthStatus health
Health status of satellite at ref time.
Definition: Xvt.hpp:157
gnsstk::CivilTime
Definition: CivilTime.hpp:55
gnsstk::TimeSystem::GPS
@ GPS
GPS system time.
gnsstk::BrcKeplerOrbit::getCrc
double getCrc() const
Definition: BrcKeplerOrbit.cpp:594
std
Definition: Angle.hpp:142
gnsstk::timeDisplay
static void timeDisplay(ostream &os, const CommonTime &t)
Definition: BrcClockCorrection.cpp:352
gnsstk::BrcKeplerOrbit::getEcc
double getEcc() const
Definition: BrcKeplerOrbit.cpp:655
gnsstk::BrcKeplerOrbit::svRelativity
double svRelativity(const CommonTime &t) const
Definition: BrcKeplerOrbit.cpp:452
gnsstk::BrcKeplerOrbit::getDn
double getDn() const
Definition: BrcKeplerOrbit.cpp:645
gnsstk::BrcKeplerOrbit::isHealthy
bool isHealthy() const
Definition: BrcKeplerOrbit.cpp:284
gnsstk::BrcKeplerOrbit::loadData
void loadData(const std::string satSysArg, const ObsID obsIDArg, const short PRNIDArg, const CommonTime beginFitArg, const CommonTime endFitArg, const CommonTime ToeArg, const short URAoeArg, const bool healthyArg, const double CucArg, const double CusArg, const double CrcArg, const double CrsArg, const double CicArg, const double CisArg, const double M0Arg, const double dnArg, const double dndotArg, const double eccArg, const double AArg, const double AhalfArg, const double AdotArg, const double OMEGA0Arg, const double i0Arg, const double wArg, const double OMEGAdotARg, const double idotArg)
General purpose means to load data into object.
Definition: BrcKeplerOrbit.cpp:136
GNSSTK_THROW
#define GNSSTK_THROW(exc)
Definition: Exception.hpp:366
gnsstk::BrcKeplerOrbit::getEndOfFitInterval
CommonTime getEndOfFitInterval() const
Definition: BrcKeplerOrbit.cpp:492
gnsstk::BrcKeplerOrbit::getCuc
double getCuc() const
Definition: BrcKeplerOrbit.cpp:604
gnsstk::GPSEllipsoid::gm
virtual double gm() const noexcept
Definition: GPSEllipsoid.hpp:77
gnsstk::BrcKeplerOrbit::getA
double getA() const
Definition: BrcKeplerOrbit.cpp:665
gnsstk::accuracy2ura
short accuracy2ura(double acc) noexcept
Definition: GPS_URA.hpp:112
gnsstk::ura2CNAVaccuracy
double ura2CNAVaccuracy(short ura)
Definition: GPS_URA.hpp:168
gnsstk::BrcKeplerOrbit::getCis
double getCis() const
Definition: BrcKeplerOrbit.cpp:584
gnsstk::BrcKeplerOrbit::getOrbitEpoch
CommonTime getOrbitEpoch() const
Definition: BrcKeplerOrbit.cpp:477
gnsstk::Xvt::Unhealthy
@ Unhealthy
Sat is marked unhealthy, do not use PVT.
Definition: Xvt.hpp:94


gnsstk
Author(s):
autogenerated on Wed Oct 25 2023 02:40:38