EngEphemeris.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 <iomanip>
44 #include <cmath>
45 
46 #include "StringUtils.hpp"
47 #include "GNSSconstants.hpp"
48 #include "MathBase.hpp"
49 #include "GPSEllipsoid.hpp"
50 #include "EngEphemeris.hpp"
51 #include "GPSWeekSecond.hpp"
52 #include "YDSTime.hpp"
53 #include "CivilTime.hpp"
54 #include "TimeSystem.hpp"
55 #include "GPS_URA.hpp"
56 #include "TimeString.hpp"
57 
58 namespace gnsstk
59 {
60  using namespace std;
61 
63  noexcept
64  {
65  haveSubframe[0] = haveSubframe[1] = haveSubframe[2] = false;
66 
67  tlm_message[0] = tlm_message[1] = tlm_message[2] = 0;
68 
69  satSys = "";
70 
71  PRNID = tracker = ASalert[0] = ASalert[1] = ASalert[2] = weeknum =
72  codeflags = health = L2Pdata = 0;
73 
74  HOWtime[0] = HOWtime[1] = HOWtime[2] = 0;
75 
76  IODC = IODE = 0;
77  Tgd = 0.0;
78 
79  isFIC = true;
80 
81  fitint = 0;
82 
83  /*
84  * AODO is initialized to 27900 to indicate that the offset
85  * data should not be relied upon.
86  * The satellite transmits a 5-bit unsigned integer that is
87  * then multiplied by 900 to get the ago of the offset data
88  * in seconds
89  * 31 * 900 = 27900, hence setting AODO to 27900 is setting
90  * the age to the maximum value
91  */
92  AODO = 27900;
93 
94  for (int j = 0; j<3; j++)
95  {
96  for (int i = 0; i<10; i++) subframeStore[j][i] = 0L;
97  }
98  }
99 
100 
101  bool EngEphemeris::operator==(const gnsstk::EngEphemeris& right) const noexcept
102  {
103  // ignored as not important for eng eph comparison
104  //haveSubframe
105  //subframeStore
106  //isFIC;
107  // EngNav has no data to compare
108  return ((tlm_message[0] == right.tlm_message[0]) &&
109  (tlm_message[1] == right.tlm_message[1]) &&
110  (tlm_message[2] == right.tlm_message[2]) &&
111  (satSys == right.satSys) &&
112  (PRNID == right.PRNID) &&
113  (tracker == right.tracker) &&
114  (HOWtime[0] == right.HOWtime[0]) &&
115  (HOWtime[1] == right.HOWtime[1]) &&
116  (HOWtime[2] == right.HOWtime[2]) &&
117  (ASalert[0] == right.ASalert[0]) &&
118  (ASalert[1] == right.ASalert[1]) &&
119  (ASalert[2] == right.ASalert[2]) &&
120  (weeknum == right.weeknum) &&
121  (codeflags == right.codeflags) &&
122  (health == right.health) &&
123  (L2Pdata == right.L2Pdata) &&
124  (IODC == right.IODC) &&
125  (IODE == right.IODE) &&
126  (AODO == right.AODO) &&
127  (fitint == right.fitint) &&
128  (Tgd == right.Tgd) &&
129  (bcClock == right.bcClock) &&
130  (orbit == right.orbit));
131  }
132 
133 
146  bool EngEphemeris::addSubframe(const uint32_t subframe[10],
147  const int gpsWeek, const short PRN,
148  const short track)
149  {
150  // Determine the subframe number
151  uint32_t SFword2 = (uint32_t) subframe[1];
152  SFword2 &= 0x00000700; // Strip all but three bit subframe ID
153  SFword2 >>= 8; // Right shift to move to lsbs;
154  short sfID = static_cast<short>( SFword2 );
155 
156  if (sfID<1 || sfID>3)
157  {
158  InvalidParameter exc("Invalid SF ID: "+StringUtils::asString(sfID));
159  GNSSTK_THROW(exc);
160  }
161 
162  // Store the subframe in the appropriate location
163  // and set the flag
164  int sfNdx = sfID - 1;
165  for (int i=0;i<10;++i) subframeStore[sfNdx][i] = subframe[i];
166  haveSubframe[sfNdx] = true;
167 
168  // Determine if all subframes are available. If so,
169  // load the data. Otherwise return and try again later.
170  // default return OK in cases where no cracking occurs
171  bool result = true;
172  if (haveSubframe[0] &&
173  haveSubframe[1] &&
174  haveSubframe[2])
175  {
176  result = unifiedConvert( gpsWeek, PRN, track );
177  }
178  return(result);
179  }
180 
181  bool EngEphemeris::addSubframeNoParity(const uint32_t subframe[10],
182  const int gpsWeek,
183  const short PRN,
184  const short track)
185  {
186  uint32_t paddedSF[10];
187  short PRNArg;
188  short trackArg;
189 
190  for (int i=0;i<10;++i)
191  {
192  paddedSF[i] = subframe[i];
193  paddedSF[i] <<= 6;
194  paddedSF[i] &= 0x3FFFFFC0; // Guarantee 2 msb and 6 lsb are zeroes
195  }
196  PRNArg = PRN;
197  trackArg = track;
198  return( addSubframe( paddedSF, gpsWeek, PRNArg, trackArg ));
199  }
200 
202  const uint32_t sf1[8], const uint32_t sf2[8], const uint32_t sf3[8],
203  const long sf1TransmitSOW, const int gpsWeek,
204  const short PRN, const short track)
205  {
206  // Need to provide a valid subframe number in the handover word.
207  // While we're at it, we'll fake the A-S bit such that it
208  // appears A-S is ON, even though we warn the user NOT to trust
209  // returns from the getASAlert() method.
210  const uint32_t sf1Lead[2] = { 0x00000000, 0x00000900 };
211  const uint32_t sf2Lead[2] = { 0x00000000, 0x00000A00 };
212  const uint32_t sf3Lead[2] = { 0x00000000, 0x00000B00 };
213 
214  // Handover word times represent the time of the leading edge of the
215  // NEXvt subframe. Therefore, HOW should always correspond to
216  // :06/:36 for SF 1
217  // :12/:42 for SF 2
218  // :18/:48 for SF 3
219  // This method hasn't a clue about the accuracy of the SOW
220  // input by the user, but it WILL enforce this relationship.
221  //long frameCount = sf1TransmitSOW / 30;
222  //long SF1HOWTime = (frameCount * 30) + 6;
223 
224  // Convert subframe 1 parameters
225  subframeStore[0][0] = sf1Lead[0];
226  subframeStore[0][1] = sf1Lead[1];
227  int i;
228  for (i=0; i<8; ++i) subframeStore[0][i+2] = sf1[i];
229  haveSubframe[0] = true;
230 
231  // Convert subframe 2 parameters
232  subframeStore[1][0] = sf2Lead[0];
233  subframeStore[1][1] = sf2Lead[1];
234  for (i=0; i<8; ++i) subframeStore[1][i+2] = sf2[i];
235  haveSubframe[1] = true;
236 
237  // Convert subframe 3 parameters
238  subframeStore[2][0] = sf3Lead[0];
239  subframeStore[2][1] = sf3Lead[1];
240  for (i=0; i<8; ++i) subframeStore[2][i+2] = sf3[i];
241  haveSubframe[2] = true;
242 
243  // Call method to crack and load the data.
244  bool result = unifiedConvert( gpsWeek, PRN, track );
245 
246  return(result);
247  }
248 
254  bool EngEphemeris::unifiedConvert( const int gpsWeek,
255  const short PRN,
256  const short track)
257  {
258  double ficked[60];
259 
260  if (!subframeConvert(subframeStore[0], gpsWeek, ficked))
261  return false;
262  tlm_message[0] = (subframeStore[0][0] >> 8) & 0x3fff;
263  HOWtime[0] = static_cast<long>( ficked[2] );
264  ASalert[0] = static_cast<short>( ficked[3] );
265  weeknum = static_cast<short>( ficked[5] );
266  codeflags = static_cast<short>( ficked[6] );
267  short accFlag = static_cast<short>( ficked[7] );
268  health = static_cast<short>( ficked[8] );
269  IODC = static_cast<short>( ldexp( ficked[9], -11 ) );
270  L2Pdata = static_cast<short>( ficked[10] );
271  Tgd = ficked[11];
272  double Toc = ficked[12];
273  double af2 = ficked[13];
274  double af1 = ficked[14];
275  double af0 = ficked[15];
276  tracker = track;
277 
278  if (!subframeConvert(subframeStore[1], gpsWeek, ficked))
279  return false;
280 
281  tlm_message[1] = (subframeStore[1][0] >> 8) & 0x3fff;
282  HOWtime[1] = static_cast<long>( ficked[2] );
283  ASalert[1] = static_cast<short>( ficked[3] );
284  IODE = static_cast<short>( ldexp( ficked[5], -11 ) );
285  double Crs = ficked[6];
286  double dn = ficked[7];
287  double M0 = ficked[8];
288  double Cuc = ficked[9];
289  double ecc = ficked[10];
290  double Cus = ficked[11];
291  double Ahalf = ficked[12];
292  double Toe = ficked[13];
293  fitint = static_cast<short>( ficked[14] );
294  AODO = static_cast<long>( ficked[15] );
295 
296 
297  if (!subframeConvert(subframeStore[2], gpsWeek, ficked))
298  return false;
299 
300  tlm_message[2] = (subframeStore[2][0] >> 8) & 0x3fff;
301  HOWtime[2] = static_cast<long>( ficked[2] );
302  ASalert[2] = static_cast<short>( ficked[3] );
303  double Cic = ficked[5];
304  double OMEGA0 = ficked[6];
305  double Cis = ficked[7];
306  double i0 = ficked[8];
307  double Crc = ficked[9];
308  double w = ficked[10];
309  double OMEGAdot = ficked[11];
310  double idot = ficked[13];
311 
312  // The system is assumed (legacy navigation message is from GPS)
313  satSys = "G";
314  PRNID = PRN;
315 
316  // The observation ID has a type of navigation, but the
317  // carrier and code types are undefined. They could be
318  // L1/L2 C/A, P, Y,.....
320 
321  bool healthy = false;
322  if (health==0)
323  healthy = true;
324  double Adot = 0.0;
325  double dnDot = 0.0;
326  double A = Ahalf * Ahalf;
327 
328  double timeDiff = Toe - HOWtime[1];
329  short toeWeek = weeknum, tocWeek = weeknum;
330  if (timeDiff < -HALFWEEK)
331  toeWeek++;
332  else if (timeDiff > HALFWEEK)
333  toeWeek--;
334  timeDiff = Toc - HOWtime[1];
335  if (timeDiff < -HALFWEEK)
336  tocWeek++;
337  else if (timeDiff > HALFWEEK)
338  tocWeek--;
339 
340  // Toe is now in CommonTime, and needs to be passed to
341  // BrcKeplerOrbit as a CommonTime variable.
342  CommonTime ToeCT = GPSWeekSecond(toeWeek, Toe, TimeSystem::GPS);
343  CommonTime TocCT = GPSWeekSecond(tocWeek, Toc, TimeSystem::GPS);
344 
345  //short fiti = static_cast<short>(ficked[14]);
346  short fitHours = getLegacyFitInterval(IODC, fitint);
347  long beginFitSOW = Toe - (fitHours/2)*3600;
348  long endFitSOW = Toe + (fitHours/2)*3600;
349  short beginFitWk = toeWeek;
350  short endFitWk = toeWeek;
351  if (beginFitSOW < 0)
352  {
353  beginFitSOW += FULLWEEK;
354  beginFitWk--;
355  }
356  CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW,
358 
359  if (endFitSOW >= FULLWEEK)
360  {
361  endFitSOW -= FULLWEEK;
362  endFitWk++;
363  }
364  CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
365 
366  orbit.loadData(satSys, obsID, PRN, beginFit, endFit, ToeCT,
367  accFlag, healthy, Cuc, Cus, Crc, Crs, Cic, Cis, M0,
368  dn, dnDot, ecc, A, Ahalf, Adot, OMEGA0, i0, w, OMEGAdot,
369  idot);
370 
371  bcClock.loadData( satSys, obsID, PRNID, TocCT,
372  accFlag, healthy, af0, af1, af2);
373 
374  return true;
375  }
376 
378  noexcept
379  {
380  try
381  {
382  return ((PRNID >= 1) && (PRNID <= 37) &&
383  (HOWtime[0] >= 0) && (HOWtime[0] <= 604784) &&
384  (HOWtime[1] >= 0) && (HOWtime[1] <= 604784) &&
385  (HOWtime[2] >= 0) && (HOWtime[2] <= 604784) &&
386  (getToc() >= 0) && (getToc() <= 604784) &&
387  (getToe() >= 0) && (getToe() <= 604784) &&
388  (getEcc() >= 0) && (getEcc() <= 0.03));
389  }
390  catch( ... )
391  {
392  return false;
393  }
394  }
395 
396  bool EngEphemeris::isData(short subframe) const
397  {
398  if ((subframe < 1) || (subframe > 3))
399  {
400  InvalidRequest exc("Subframe "+StringUtils::asString(subframe)+
401  " is not a valid ephemeris subframe.");
402  GNSSTK_THROW(exc);
403  }
404 
405  return haveSubframe[subframe-1];
406  }
407 
409  {
410  if (haveSubframe[0] && haveSubframe[1] && haveSubframe[2])
411  {
412  return ((IODC & 0xff) == (IODE & 0xff));
413  }
414  return false;
415  }
416 
417  void EngEphemeris::setAccuracy(double acc)
418  {
419  if( acc < 0 )
420  {
421  InvalidParameter exc("SV Accuracy of " + StringUtils::asString(acc) +
422  " meters is invalid.");
423  GNSSTK_THROW(exc);
424  }
425  orbit.setAccuracy(acc);
426  }
427 
429  {
430  return getFitInterval(getIODC(), getFitInt());
431  }
432 
437  short EngEphemeris :: getFitInterval(short iodc, short fiti)
438  {
439  /* check the IODC */
440  if (iodc < 0 || iodc > 1023)
441  {
442  /* error in iodc, return minimum fit */
443  return 4;
444  }
445 
446  if ((((fiti == 0) &&
447  (iodc & 0xFF) < 240) || (iodc & 0xFF) > 255 ))
448  {
449  /* fit interval of 4 hours */
450  return 4;
451  }
452  else if (fiti == 1)
453  {
454  if( ((iodc & 0xFF) < 240 || (iodc & 0xFF) > 255))
455  {
456  /* fit interval of 6 hours */
457  return 6;
458  }
459  else if(iodc >=240 && iodc <=247)
460  {
461  /* fit interval of 8 hours */
462  return 8;
463  }
464  else if((iodc >= 248 && iodc <= 255) || iodc == 496)
465  {
466  /* fit interval of 14 hours */
467  return 14;
468  }
469  // Revised in IS-GPS-200 Revision D for Block IIR/IIR-M/IIF
470  else if((iodc >= 497 && iodc <=503) || (iodc >= 1021 && iodc <= 1023))
471  {
472  /* fit interval of 26 hours */
473  return 26;
474  }
475  else if(iodc >= 504 && iodc <=510)
476  {
477  /* fit interval of 50 hours */
478  return 50;
479  }
480  else if(iodc == 511 || (iodc >= 752 && iodc <= 756))
481  {
482  /* fit interval of 74 hours */
483  return 74;
484  }
485  /* NOTE:
486  * The following represents old fit intervals for Block II
487  * (not IIA) and is present only in old versions of the
488  * ICD-GPS-200 Rev. C. Please do not remove them as there
489  * are still people that may want to process old Block II
490  * data and none of the IODC intervals overlap (so far) so
491  * there is no need to remove them.
492  */
493  else if(iodc >= 757 && iodc <= 763)
494  {
495  /* fit interval of 98 hours */
496  return 98;
497  }
498  else if((iodc >= 764 && iodc <=767) || (iodc >=1008 && iodc <=1010))
499  {
500  /* fit interval of 122 hours */
501  return 122;
502  }
503  else if(iodc >= 1011 && iodc <=1020)
504  {
505  /* fit interval of 146 hours */
506  return 146;
507  }
508  else
509  {
510  /* error in the iodc or ephemeris, return minimum
511  fit */
512  return 4;
513  }
514  }
515  else
516  {
517  /* error in ephemeris/iodc, return minimum fit */
518  return 4;
519  }
520 
521  return 0; // never reached
522  }
523 
524 
526  {
527  Xvt sv;
528 
529  Xvt xv = orbit.svXvt(t);
530 
531  sv.x = xv.x;
532  sv.v = xv.v;
533 
534  sv.clkbias = bcClock.svClockBias(t);
535  sv.relcorr = orbit.svRelativity(t);
536 
537  sv.clkdrift = bcClock.svClockDrift(t);
538 
539  return sv;
540  }
541 
542  double EngEphemeris::svRelativity(const CommonTime& t) const
543  {
544  return orbit.svRelativity(t);
545  }
546 
547  double EngEphemeris::svClockBias(const CommonTime& t) const
548  {
549  return bcClock.svClockBias(t);
550  }
551 
552  double EngEphemeris::svClockDrift(const CommonTime& t) const
553  {
554  return bcClock.svClockDrift(t);
555  }
556 
557  unsigned EngEphemeris::getTLMMessage(short subframe) const
558  {
559  if (!haveSubframe[subframe-1])
560  {
561  InvalidRequest exc("Subframe "+StringUtils::asString(subframe)+
562  " not stored.");
563  GNSSTK_THROW(exc);
564  }
565  return tlm_message[subframe-1];
566  }
567 
569  {
570  CommonTime toReturn;
571  try
572  {
573  toReturn = GPSWeekSecond(getFullWeek(), static_cast<double>(getTot()),
575  }
576  catch (InvalidRequest& ire)
577  {
578  GNSSTK_RETHROW(ire);
579  }
580  catch (Exception& exc)
581  {
582  InvalidRequest ire(exc);
583  GNSSTK_THROW(ire);
584  }
585  return toReturn;
586  }
587 
589  {
590  return bcClock.getEpochTime();
591  }
592 
594  {
595  return orbit.getOrbitEpoch();
596  }
597 
599  {
600  if(!orbit.hasData())
601  {
602  InvalidRequest exc("getOrbit(): Required Orbit data not stored.");
603  GNSSTK_THROW(exc);
604  }
605  return (orbit);
606  }
607 
609  {
610  if(!bcClock.hasData())
611  {
612  InvalidRequest exc("getClock(): Required Clock Correction data not"
613  " stored.");
614  GNSSTK_THROW(exc);
615  }
616  return (bcClock);
617  }
618 
620  {
621  if(!haveSubframe[0])
622  {
623  InvalidRequest exc("getPRNID(): Required subframe 1 not stored.");
624  GNSSTK_THROW(exc);
625  }
626  return PRNID;
627  }
628 
630  {
631  if(!haveSubframe[0])
632  {
633  InvalidRequest exc("getTracker(): Required subframe 1 not stored.");
634  GNSSTK_THROW(exc);
635  }
636  return tracker;
637  }
638 
639  double EngEphemeris::getHOWTime(short subframe) const
640  {
641  if (!haveSubframe[subframe-1])
642  {
643  InvalidRequest exc("getHOWTime(): Subframe "
644  +StringUtils::asString(subframe)+" not stored.");
645  GNSSTK_THROW(exc);
646  }
647  // This return as a double is necessary for sets into CommonTime
648  // to not get confused. Ints are Zcounts whereas doubles are seconds.
649  // This should still return a double after CommonTime->CommonTime
650  // conversion, for backwards compatibility. [DR]
651  return static_cast<double>(HOWtime[subframe-1]);
652  }
653 
654  short EngEphemeris::getASAlert(short subframe) const
655  {
656  if (!haveSubframe[subframe-1])
657  {
658  InvalidRequest exc("getASAlert(): Subframe "
659  +StringUtils::asString(subframe)+" not stored.");
660  GNSSTK_THROW(exc);
661  }
662  return ASalert[subframe-1];
663  }
664 
666  {
667  if (!haveSubframe[0])
668  {
669  InvalidRequest exc("getFullWeek(): Required subframe 1 not stored.");
670  GNSSTK_THROW(exc);
671  }
672  return weeknum;
673  }
674 
676  {
677  if (!haveSubframe[0])
678  {
679  InvalidRequest exc("getCodeFlags(): Required subframe 1 not stored.");
680  GNSSTK_THROW(exc);
681  }
682  return codeflags;
683  }
684 
686  {
687  if (!haveSubframe[0])
688  {
689  InvalidRequest exc("getAccuracy(): Required subframe 1 not stored.");
690  GNSSTK_THROW(exc);
691  }
692  return orbit.getAccuracy();
693  }
694 
696  {
697  if (!haveSubframe[0])
698  {
699  InvalidRequest exc("getAccFlag(): Required subframe 1 not stored.");
700  GNSSTK_THROW(exc);
701  }
702  return orbit.getURAoe();
703  }
704 
706  {
707  if (!haveSubframe[0])
708  {
709  InvalidRequest exc("getHealth(): Required subframe 1 not stored.");
710  GNSSTK_THROW(exc);
711  }
712  return health;
713  }
714 
716  {
717  if (!haveSubframe[0])
718  {
719  InvalidRequest exc("getL2Pdata(): Required subframe 1 not stored.");
720  GNSSTK_THROW(exc);
721  }
722  return L2Pdata;
723  }
724 
725  short EngEphemeris::getIODC() const
726  {
727  if (!haveSubframe[0])
728  {
729  InvalidRequest exc("getIODC(): Required subframe 1 not stored.");
730  GNSSTK_THROW(exc);
731  }
732  return static_cast<short>(IODC);
733  }
734 
735  short EngEphemeris::getIODE() const
736  {
737  if (!haveSubframe[1])
738  {
739  InvalidRequest exc("getIODE(): Required subframe 2 not stored.");
740  GNSSTK_THROW(exc);
741  }
742  return static_cast<short>(IODE);
743  }
744 
746  {
747  if (!haveSubframe[1])
748  {
749  InvalidRequest exc("getAODO(): Required subframe 2 not stored.");
750  GNSSTK_THROW(exc);
751  }
752  return AODO;
753  }
754 
755  double EngEphemeris::getToc() const
756  {
757  if (!haveSubframe[0])
758  {
759  InvalidRequest exc("getToc(): Required subframe 1 not stored.");
760  GNSSTK_THROW(exc);
761  }
762  return bcClock.getToc();
763  }
764 
765  double EngEphemeris::getAf0() const
766  {
767  if (!haveSubframe[0])
768  {
769  InvalidRequest exc("getAf0(): Required subframe 1 not stored.");
770  GNSSTK_THROW(exc);
771  }
772  return bcClock.getAf0();
773  }
774 
775  double EngEphemeris::getAf1() const
776  {
777  if (!haveSubframe[0])
778  {
779  InvalidRequest exc("getAf1(): Required subframe 1 not stored.");
780  GNSSTK_THROW(exc);
781  }
782  return bcClock.getAf1();
783  }
784 
785  double EngEphemeris::getAf2() const
786  {
787  if (!haveSubframe[0])
788  {
789  InvalidRequest exc("getAf1(): Required subframe 1 not stored.");
790  GNSSTK_THROW(exc);
791  }
792  return bcClock.getAf2();
793  }
794 
795  double EngEphemeris::getTgd() const
796  {
797  if (!haveSubframe[0])
798  {
799  InvalidRequest exc("getTgd(): Required subframe 1 not stored.");
800  GNSSTK_THROW(exc);
801  }
802  return Tgd;
803  }
804 
805  double EngEphemeris::getCus() const
806  {
807  if (!haveSubframe[1])
808  {
809  InvalidRequest exc("getCus(): Required subframe 2 not stored.");
810  GNSSTK_THROW(exc);
811  }
812  return orbit.getCus();
813  }
814 
815  double EngEphemeris::getCrs() const
816  {
817  if (!haveSubframe[1])
818  {
819  InvalidRequest exc("getCrs(): Required subframe 2 not stored.");
820  GNSSTK_THROW(exc);
821  }
822  return orbit.getCrs();
823  }
824 
825  double EngEphemeris::getCis() const
826  {
827  if (!haveSubframe[2])
828  {
829  InvalidRequest exc("getCis(): Required subframe 3 not stored.");
830  GNSSTK_THROW(exc);
831  }
832  return orbit.getCis();
833  }
834 
835  double EngEphemeris::getCrc() const
836  {
837  if (!haveSubframe[2])
838  {
839  InvalidRequest exc("getCrc(): Required subframe 3 not stored.");
840  GNSSTK_THROW(exc);
841  }
842  return orbit.getCrc();
843  }
844 
845  double EngEphemeris::getCuc() const
846  {
847  if (!haveSubframe[1])
848  {
849  InvalidRequest exc("getCuc(): Required subframe 2 not stored.");
850  GNSSTK_THROW(exc);
851  }
852  return orbit.getCuc();
853  }
854 
855  double EngEphemeris::getCic() const
856  {
857  if (!haveSubframe[2])
858  {
859  InvalidRequest exc("getCic(): Required subframe 3 not stored.");
860  GNSSTK_THROW(exc);
861  }
862  return orbit.getCic();
863  }
864 
865  double EngEphemeris::getToe() const
866  {
867  if (!haveSubframe[1])
868  {
869  InvalidRequest exc("getToe(): Required subframe 2 not stored.");
870  GNSSTK_THROW(exc);
871  }
872  return orbit.getToe();
873  }
874 
875  double EngEphemeris::getM0() const
876  {
877  if (!haveSubframe[1])
878  {
879  InvalidRequest exc("getM0(): Required subframe 2 not stored.");
880  GNSSTK_THROW(exc);
881  }
882  return orbit.getM0();
883  }
884 
885  double EngEphemeris::getDn() const
886  {
887  if (!haveSubframe[1])
888  {
889  InvalidRequest exc("getDn(): Required subframe 2 not stored.");
890  GNSSTK_THROW(exc);
891  }
892  return orbit.getDn();
893  }
894 
895  double EngEphemeris::getEcc() const
896  {
897  if (!haveSubframe[1])
898  {
899  InvalidRequest exc("getEcc(): Required subframe 2 not stored.");
900  GNSSTK_THROW(exc);
901  }
902  return orbit.getEcc();
903  }
904 
905  double EngEphemeris::getAhalf() const
906  {
907  if (!haveSubframe[1])
908  {
909  InvalidRequest exc("getAhalf(): Required subframe 2 not stored.");
910  GNSSTK_THROW(exc);
911  }
912  return orbit.getAhalf();
913  }
914 
915  double EngEphemeris::getA() const
916  {
917  if (!haveSubframe[1])
918  {
919  InvalidRequest exc("getA(): Required subframe 2 not stored.");
920  GNSSTK_THROW(exc);
921  }
922  return orbit.getA();
923  }
924 
925  double EngEphemeris::getOmega0() const
926  {
927  if (!haveSubframe[2])
928  {
929  InvalidRequest exc("getOmega0(): Required subframe 3 not stored.");
930  GNSSTK_THROW(exc);
931  }
932  return orbit.getOmega0();
933  }
934 
935  double EngEphemeris::getI0() const
936  {
937  if (!haveSubframe[2])
938  {
939  InvalidRequest exc("getI0(): Required subframe 3 not stored.");
940  GNSSTK_THROW(exc);
941  }
942  return orbit.getI0();
943  }
944 
945  double EngEphemeris::getW() const
946  {
947  if (!haveSubframe[2])
948  {
949  InvalidRequest exc("getW(): Required subframe 3 not stored.");
950  GNSSTK_THROW(exc);
951  }
952  return orbit.getW();
953  }
954 
956  {
957  if (!haveSubframe[2])
958  {
959  InvalidRequest exc("getOmegaDot(): Required subframe 3 not stored.");
960  GNSSTK_THROW(exc);
961  }
962  return orbit.getOmegaDot();
963  }
964 
965  double EngEphemeris::getIDot() const
966  {
967  if (!haveSubframe[2])
968  {
969  InvalidRequest exc("getIDot(): Required subframe 3 not stored.");
970  GNSSTK_THROW(exc);
971  }
972  return orbit.getIDot();
973  }
974 
976  {
977  if (!haveSubframe[1])
978  {
979  InvalidRequest exc("getFitInt(): Required subframe 2 not stored.");
980  GNSSTK_THROW(exc);
981  }
982  return fitint;
983  }
984 
985  long EngEphemeris::getTot() const
986  {
987  if(!haveSubframe[0])
988  {
989  InvalidRequest exc("getTot(): Required subframe 1 not stored.");
990  GNSSTK_THROW(exc);
991  }
992  if(!haveSubframe[1])
993  {
994  InvalidRequest exc("getTot(): Required subframe 2 not stored.");
995  GNSSTK_THROW(exc);
996  }
997  if(!haveSubframe[2])
998  {
999  InvalidRequest exc("getTot(): Required subframe 3 not stored.");
1000  GNSSTK_THROW(exc);
1001  }
1002 
1003  // MSVC
1004 #ifdef _MSC_VER
1005  long foo = static_cast<long>( getHOWTime(1) < getHOWTime(2) )
1006  ? getHOWTime(1) : getHOWTime(2);
1007  foo = ( foo < getHOWTime(3) ) ? foo : getHOWTime(3) ;
1008 #else
1009  long foo =
1010  static_cast<long>( std::min( getHOWTime(1),
1011  std::min(getHOWTime(2), getHOWTime(3))));
1012 #endif
1013  // The ephemeris comes on 30 second boundaries, so...
1014  foo/=30;
1015  foo*=30;
1016  return foo;
1017  }
1018 
1020  const std::string satSysArg, const unsigned short tlm[3],
1021  const long how[3], const short asalert[3],
1022  const short Tracker, const short prn,
1023  const short fullweek, const short cflags, const short acc,
1024  const short svhealth, const short iodc, const short l2pdata,
1025  const long aodo, const double tgd, const double toc,
1026  const double Af2, const double Af1, const double Af0,
1027  const short iode, const double crs, const double Dn,
1028  const double m0, const double cuc, const double Ecc,
1029  const double cus, const double ahalf, const double toe,
1030  const short fitInt, const double cic, const double Omega0,
1031  const double cis, const double I0, const double crc,
1032  const double W, const double OmegaDot, const double IDot )
1033  {
1034  PRNID = prn;
1035  tracker = Tracker;
1036  for (int i=0; i<3; i++)
1037  {
1038  tlm_message[i] = tlm[i];
1039  HOWtime[i] = how[i];
1040  ASalert[i] = asalert[i];
1041  }
1042  weeknum = fullweek;
1043  codeflags = cflags;
1044  short accFlag = acc;
1045  //double accuracy = gnsstk::ura2accuracy(accFlag);
1046  health = svhealth;
1047  L2Pdata = l2pdata;
1048  IODC = iodc;
1049  IODE = iode;
1050  AODO = aodo;
1051  fitint = fitInt;
1052  Tgd = tgd;
1053  satSys = satSysArg;
1054 
1055  satSys = "G";
1056 
1057  // The observation ID has a type of navigation, but the
1058  // carrier and code types are undefined. They could be
1059  // L1/L2 C/A, P, Y,.....
1061  try
1062  {
1063  CommonTime toeCT = GPSWeekSecond(weeknum, toe, TimeSystem::GPS);
1064  CommonTime tocCT = GPSWeekSecond(weeknum, toc, TimeSystem::GPS);
1065 
1066  double A = ahalf*ahalf;
1067  double dndot = 0.0;
1068  double Adot = 0.0;
1069  short fitHours = getLegacyFitInterval(IODC, fitint);
1070  long beginFitSOW = toe - (fitHours/2)*3600;
1071  long endFitSOW = toe + (fitHours/2)*3600;
1072  short beginFitWk = weeknum;
1073  short endFitWk = weeknum;
1074  if (beginFitSOW < 0)
1075  {
1076  beginFitSOW += FULLWEEK;
1077  beginFitWk--;
1078  }
1079  CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW,
1080  TimeSystem::GPS);
1081  if (endFitSOW >= FULLWEEK)
1082  {
1083  endFitSOW += FULLWEEK;
1084  endFitWk++;
1085  }
1086  CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW,
1087  TimeSystem::GPS);
1088 
1089  orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, toeCT,
1090  accFlag, (health == 0), cuc, cus, crc, crs, cic, cis,
1091  m0, Dn, dndot, Ecc, A, ahalf, Adot, Omega0, I0, W,
1092  OmegaDot, IDot);
1093 
1094  bcClock.loadData( satSys, obsID, PRNID, tocCT,
1095  accFlag, (health == 0), Af0, Af1, Af2);
1096  haveSubframe[0] = true;
1097  haveSubframe[1] = true;
1098  haveSubframe[2] = true;
1099  }
1100  catch (Exception& exc)
1101  {
1102  InvalidRequest ire(exc);
1103  GNSSTK_THROW(ire);
1104  }
1105  return *this;
1106  }
1107 
1108  EngEphemeris& EngEphemeris::setSF1( unsigned tlm, double how, short asalert,
1109  short fullweek, short cflags, short acc,
1110  short svhealth, short iodc,
1111  short l2pdata, double tgd, double toc,
1112  double Af2, double Af1, double Af0,
1113  short Tracker, short prn )
1114  {
1115  tlm_message[0] = tlm;
1116  HOWtime[0] = static_cast<long>( how );
1117  ASalert[0] = asalert;
1118  weeknum = fullweek;
1119  codeflags = cflags;
1120  accFlagTmp = acc;
1121  health = svhealth;
1122  IODC = iodc;
1123  L2Pdata = l2pdata;
1124  Tgd = tgd;
1125  tracker = Tracker;
1126  PRNID = prn;
1127  bool healthy = false;
1128  if (health == 0) healthy = true;
1129 
1130  double timeDiff = toc - HOWtime[0];
1131  short tocWeek = fullweek;
1132  if (timeDiff < -HALFWEEK)
1133  tocWeek++;
1134  else if (timeDiff > HALFWEEK)
1135  tocWeek--;
1136  try
1137  {
1138  CommonTime tocCT = GPSWeekSecond(tocWeek, toc, TimeSystem::GPS);
1139 
1140  // The system is assumed (legacy navigation message is from GPS)
1141  satSys = "G";
1142 
1143  // The observation ID has a type of navigation, but the
1144  // carrier and code types are undefined. They could be
1145  // L1/L2 C/A, P, Y,.....
1147 
1148  bcClock.loadData( satSys, obsID, PRNID, tocCT,
1149  accFlagTmp, healthy, Af0, Af1, Af2);
1150  haveSubframe[0] = true;
1151  }
1152  catch (Exception& exc)
1153  {
1154  haveSubframe[0] = false;
1155  InvalidRequest ire(exc);
1156  GNSSTK_THROW(ire);
1157  }
1158  return *this;
1159  }
1160 
1161  EngEphemeris& EngEphemeris::setSF2( unsigned tlm, double how, short asalert,
1162  short iode, double crs, double Dn,
1163  double m0, double cuc, double Ecc,
1164  double cus, double ahalf, double toe,
1165  short fitInt )
1166  {
1167  tlm_message[1] = tlm;
1168  HOWtime[1] = static_cast<long>( how );
1169  ASalert[1] = asalert;
1170  IODE = iode;
1171  fitint = fitInt;
1172 
1173  if (!haveSubframe[0])
1174  {
1175  InvalidRequest exc("Need to load subframe 1 before subframe 2");
1176  GNSSTK_THROW(exc);
1177  }
1178  bool healthy = false;
1179  if (health == 0)
1180  healthy = true;
1181 
1182  double timeDiff = toe - HOWtime[1];
1183  short toeWeek = weeknum;
1184  if (timeDiff < -HALFWEEK)
1185  toeWeek++;
1186  else if (timeDiff > HALFWEEK)
1187  toeWeek--;
1188 
1189  // The observation ID has a type of navigation, but the
1190  // carrier and code types are undefined. They could be
1191  // L1/L2 C/A, P, Y,.....
1193 
1194  short accFlag = accFlagTmp; // accFlagTmp set in setSF1( )
1195  //local variables in SF3 that are needed to load SF2
1196  double crc = 0.0;
1197  double cis = 0.0;
1198  double cic = 0.0;
1199  double Omega0 = 0.0;
1200  double I0 = 0.0;
1201  double W = 0.0;
1202  double OmegaDot = 0.0;
1203  double IDot = 0.0;
1204  //also need locals for modernized nav quantaties not in SF2 or SF3
1205  double A = ahalf*ahalf; // TEMP fix BWT
1206  double dndot = 0.0;
1207  double Adot = 0.0;
1208  try
1209  {
1210  short fitHours = getLegacyFitInterval(IODC, fitint);
1211  long beginFitSOW = toe - (fitHours/2)*3600.0;
1212  long endFitSOW = toe + (fitHours/2)*3600.0;
1213  short beginFitWk = weeknum;
1214  short endFitWk = weeknum;
1215  if (beginFitSOW < 0)
1216  {
1217  beginFitSOW += FULLWEEK;
1218  beginFitWk--;
1219  }
1220  CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW,
1221  TimeSystem::GPS);
1222  if (endFitSOW >= FULLWEEK)
1223  {
1224  endFitSOW += FULLWEEK;
1225  endFitWk++;
1226  }
1227  CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW,
1228  TimeSystem::GPS);
1229 
1230  CommonTime toeCT = GPSWeekSecond(toeWeek, toe, TimeSystem::GPS);
1231 
1232  orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, toeCT,
1233  accFlag, healthy, cuc, cus, crc, crs, cic, cis, m0, Dn,
1234  dndot, Ecc, A, ahalf, Adot, Omega0, I0, W, OmegaDot,
1235  IDot);
1236  haveSubframe[1] = true;
1237  }
1238  catch (Exception& exc)
1239  {
1240  haveSubframe[1] = false;
1241  InvalidRequest ire(exc);
1242  GNSSTK_THROW(ire);
1243  }
1244  return *this;
1245  }
1246 
1247  EngEphemeris& EngEphemeris::setSF3( unsigned tlm, double how, short asalert,
1248  double cic, double Omega0, double cis,
1249  double I0, double crc, double W,
1250  double OmegaDot, double IDot )
1251  {
1252  tlm_message[2] = tlm;
1253  HOWtime[2] = static_cast<long>( how );
1254  ASalert[2] = asalert;
1255 
1256  if (!haveSubframe[1])
1257  {
1258  InvalidRequest exc("Need to load subframe 2 before subframe 3");
1259  GNSSTK_THROW(exc);
1260  }
1261  bool healthy = false;
1262  if (health == 0)
1263  healthy = true;
1264 
1265  double timeDiff = orbit.getToe() - HOWtime[2];
1266  short toeWeek = weeknum;
1267  if (timeDiff < -HALFWEEK)
1268  toeWeek++;
1269  else if (timeDiff > HALFWEEK)
1270  toeWeek--;
1271 
1272  // The observation ID has a type of navigation, but the
1273  // carrier and code types are undefined. They could be
1274  // L1/L2 C/A, P, Y,.....
1276 
1277  short accFlag = 0;
1278  double toe = 0.0;
1279  double cuc = 0.0;
1280  double cus = 0.0;
1281  double crs = 0.0;
1282  double m0 = 0.0;
1283  double Dn = 0.0;
1284  double dndot = 0.0;
1285  double Ecc = 0.0;
1286  double A = 0.0;
1287  double ahalf = 0.0;
1288  double Adot = 0.0;
1289  CommonTime beginFit;
1290  CommonTime endFit;
1291  try
1292  {
1293  accFlag = orbit.getURAoe();
1294  toe = orbit.getToe();
1295  cuc = orbit.getCuc();
1296  cus = orbit.getCus();
1297  dndot = orbit.getDnDot();
1298  A = orbit.getA();
1299  Adot = orbit.getAdot();
1300  crs = orbit.getCrs();
1301  m0 = orbit.getM0();
1302  Dn = orbit.getDn();
1303  Ecc = orbit.getEcc();
1304  ahalf = orbit.getAhalf();
1305  beginFit = orbit.getBeginningOfFitInterval();
1306  endFit = orbit.getEndOfFitInterval();
1307  }
1308  catch (Exception)
1309  {
1310  //Should not get to this point because of the
1311  //if(!haveSubFrame[1]) check above.
1312  haveSubframe[1] = false;
1313  haveSubframe[2] = false;
1314  return *this;
1315  }
1316  try
1317  {
1318  CommonTime toeCT = GPSWeekSecond(toeWeek, toe, TimeSystem::GPS);
1319 
1320  orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, toeCT,
1321  accFlag, healthy, cuc, cus, crc, crs, cic, cis, m0, Dn,
1322  dndot, Ecc, A, ahalf, Adot, Omega0, I0, W, OmegaDot,
1323  IDot);
1324 
1325  haveSubframe[2] = true;
1326  }
1327  catch (Exception& exc)
1328  {
1329  haveSubframe[2] = false;
1330  }
1331  return *this;
1332  }
1333 
1334  void EngEphemeris::setFIC(const bool arg)
1335  {
1336  isFIC = arg;
1337  }
1338 
1339  static void timeDisplay( ostream & os, const CommonTime& t )
1340  {
1341  // Convert to CommonTime struct from GPS wk,SOW to M/D/Y, H:M:S.
1342  GPSWeekSecond dummyTime;
1343  dummyTime = GPSWeekSecond(t);
1344  os << setw(4) << dummyTime.week << "(";
1345  os << setw(4) << (dummyTime.week & 0x03FF) << ") ";
1346  os << setw(6) << setfill(' ') << dummyTime.sow << " ";
1347 
1348  switch (dummyTime.getDayOfWeek())
1349  {
1350  case 0: os << "Sun-0"; break;
1351  case 1: os << "Mon-1"; break;
1352  case 2: os << "Tue-2"; break;
1353  case 3: os << "Wed-3"; break;
1354  case 4: os << "Thu-4"; break;
1355  case 5: os << "Fri-5"; break;
1356  case 6: os << "Sat-6"; break;
1357  default: break;
1358  }
1359  os << " " << (static_cast<YDSTime>(t)).printf("%3j %5.0s ")
1360  << (static_cast<CivilTime>(t)).printf("%02m/%02d/%04Y %02H:%02M:%02S");
1361  }
1362 
1363  static void shortcut(ostream & os, const long HOW )
1364  {
1365  short DOW, hour, min, sec;
1366  long SOD, SOW;
1367  short SOH;
1368 
1369  SOW = static_cast<long>( HOW );
1370  DOW = static_cast<short>( SOW / SEC_PER_DAY );
1371  SOD = SOW - static_cast<long>( DOW * SEC_PER_DAY );
1372  hour = static_cast<short>( SOD/3600 );
1373 
1374  SOH = static_cast<short>( SOD - (hour*3600) );
1375  min = SOH/60;
1376 
1377  sec = SOH - min * 60;
1378  switch (DOW)
1379  {
1380  case 0: os << "Sun-0"; break;
1381  case 1: os << "Mon-1"; break;
1382  case 2: os << "Tue-2"; break;
1383  case 3: os << "Wed-3"; break;
1384  case 4: os << "Thu-4"; break;
1385  case 5: os << "Fri-5"; break;
1386  case 6: os << "Sat-6"; break;
1387  default: break;
1388  }
1389 
1390  os << ":" << setfill('0')
1391  << setw(2) << hour
1392  << ":" << setw(2) << min
1393  << ":" << setw(2) << sec
1394  << setfill(' ');
1395  }
1396 
1397 
1398  void EngEphemeris :: dumpTerse(ostream& s) const
1399  {
1400 
1401  // Check if the subframes have been loaded before attempting
1402  // to dump them.
1403  if (!haveSubframe[0] || !haveSubframe[1] || !haveSubframe[2])
1404  {
1405  InvalidRequest exc("Need to load subframes 1,2 and 3");
1406  GNSSTK_THROW(exc);
1407  }
1408 
1409  ios::fmtflags oldFlags = s.flags();
1410 
1411  s.setf(ios::fixed, ios::floatfield);
1412  s.setf(ios::right, ios::adjustfield);
1413  s.setf(ios::uppercase);
1414  s.precision(0);
1415  s.fill(' ');
1416 
1417  s << setw(2) << PRNID << " ! ";
1418 
1419  string tform = "%3j %02H:%02M:%02S";
1420 
1421  s << printTime(getTransmitTime(), tform) << " ! ";
1422  s << printTime(bcClock.getEpochTime(), tform) << " ! ";
1423  s << printTime(orbit.getEndOfFitInterval(), tform) << " ! ";
1424 
1425  s << setw(4) << setprecision(1) << getAccuracy() << " ! ";
1426  s << "0x" << setfill('0') << hex << setw(3) << IODC << " ! ";
1427  s << "0x" << setfill('0') << setw(2) << health;
1428  s << setfill(' ') << dec;
1429  s << " " << setw(2) << health << " ! ";
1430 
1431  s << endl;
1432  s.flags(oldFlags);
1433 
1434  } // end of SF123::dumpTerse()
1435 
1436 
1437 
1438  void EngEphemeris :: dump(ostream& s) const
1439  {
1440 
1441 
1442  // Check if the subframes have been loaded before attempting
1443  // to dump them.
1444  if (!haveSubframe[0] || !haveSubframe[1] || !haveSubframe[2])
1445  {
1446  InvalidRequest exc("Need to load subframes 1,2 and 3");
1447  GNSSTK_THROW(exc);
1448  }
1449 
1450  ios::fmtflags oldFlags = s.flags();
1451 
1452  s.setf(ios::fixed, ios::floatfield);
1453  s.setf(ios::right, ios::adjustfield);
1454  s.setf(ios::uppercase);
1455  s.precision(0);
1456  s.fill(' ');
1457 
1458  s << "****************************************************************"
1459  << "************" << endl
1460  << "Broadcast Ephemeris (Engineering Units)";
1461  if(isFIC)
1462  {
1463  s << " -FIC" << endl;
1464  }
1465  else
1466  {
1467  s << " -RINEX" << endl;
1468  }
1469  s << endl;
1470  s << "PRN : " << setw(2) << PRNID << endl;
1471  s << endl;
1472 
1473 
1474  s << " Week(10bt) SOW DOW UTD SOD"
1475  << " MM/DD/YYYY HH:MM:SS\n";
1476  s << "Clock Epoch: ";
1477 
1478  timeDisplay(s, bcClock.getEpochTime());
1479  s << endl;
1480  s << "Eph Epoch: ";
1481  timeDisplay(s, orbit.getOrbitEpoch());
1482  s << endl;
1483 /*
1484  #if 0
1485  // FIX when moved from sf123, the tot got zapped.. because in
1486  // order for EngEphemeris to be able to use such a thing, it
1487  // needs to be pulled out of as-broadcast bits somehow.
1488  s << "Transmit time:" << setw(4) << weeknum << ", sow=" << Tot.GPSsecond() << endl
1489  << "Fit interval flag : " << setw(2) << fitint
1490  << " (" << fitintlen << " hours)" << endl;
1491  #elsif 0
1492  s << "Transmit time:" << setw(4) << weeknum << endl
1493  << "Fit interval flag : " << setw(2) << fitint
1494  << " (" << getFitInt() << " hours)" << endl;
1495  #endif
1496  // nuts to the above, let's just make it look like navdump output
1497  */
1498  s << "Transmit Time:";
1499  timeDisplay(s, getTransmitTime());
1500  s << endl;
1501  s << "Fit interval flag : " << fitint << endl;
1502  if(isFIC)
1503  {
1504  s << endl
1505  << " SUBFRAME OVERHEAD"
1506  << endl
1507  << endl
1508  << " SOW DOW:HH:MM:SS IOD ALERT A-S\n";
1509  for (int i=0;i<3;i++)
1510  {
1511  s << "SF" << setw(1) << (i+1)
1512  << " HOW: " << setw(7) << HOWtime[i]
1513  << " ";
1514 
1515  shortcut( s, HOWtime[i]);
1516  if (i==0)
1517  s << " ";
1518  else
1519  s << " ";
1520 
1521  s << "0x" << setfill('0') << hex;
1522 
1523  if (i==0)
1524  s << setw(3) << IODC;
1525  else
1526  s << setw(2) << IODE;
1527 
1528  s << dec << " " << setfill(' ');
1529 
1530  if (ASalert[i] & 0x0002) // "Alert" bit handling
1531  s << "1 ";
1532  else
1533  s << "0 ";
1534 
1535  if (ASalert[i] & 0x0001) // A-S flag handling
1536  s << " on";
1537  else
1538  s << "off";
1539  s << endl;
1540  }
1541  }
1542  else
1543  {
1544  s << endl;
1545  s << "IODC: 0x"
1546  << setfill('0') << hex;
1547  s << setw(3) << IODC << endl;
1548  s << "IODE: 0x"
1549  << setfill('0') << hex;
1550  s << setw(2) << IODE << endl;
1551  }
1552  s.setf(ios::scientific, ios::floatfield);
1553  s.precision(8);
1554  s.fill(' ');
1555 
1556  s << endl
1557  << " CLOCK"
1558  << endl
1559  << endl
1560  << "Bias T0: " << setw(16) << bcClock.getAf0() << " sec" << endl
1561  << "Drift: " << setw(16) << bcClock.getAf1() << " sec/sec" << endl
1562  << "Drift rate: " << setw(16) << bcClock.getAf2() << " sec/(sec**2)" << endl
1563  << "Group delay: " << setw(16) << Tgd << " sec" << endl;
1564 
1565  s << endl
1566  << " ORBIT PARAMETERS"
1567  << endl
1568  << endl
1569  << "Semi-major axis: " << setw(16) << orbit.getAhalf() << " m**.5" << endl
1570  << "Motion correction: " << setw(16) << orbit.getDn() << " rad/sec"
1571  << endl
1572  << "Eccentricity: " << setw(16) << orbit.getEcc() << endl
1573  << "Arg of perigee: " << setw(16) << orbit.getW() << " rad" << endl
1574  << "Mean anomaly at epoch: " << setw(16) << orbit.getM0() << " rad" << endl
1575  << "Right ascension: " << setw(16) << orbit.getOmega0() << " rad "
1576  << setw(16) << orbit.getOmegaDot() << " rad/sec" << endl
1577  << "Inclination: " << setw(16) << orbit.getI0() << " rad "
1578  << setw(16) << orbit.getIDot() << " rad/sec" << endl;
1579 
1580  s << endl
1581  << " HARMONIC CORRECTIONS"
1582  << endl
1583  << endl
1584  << "Radial Sine: " << setw(16) << orbit.getCrs() << " m Cosine: "
1585  << setw(16) << orbit.getCrc() << " m" << endl
1586  << "Inclination Sine: " << setw(16) << orbit.getCis() << " rad Cosine: "
1587  << setw(16) << orbit.getCic() << " rad" << endl
1588  << "In-track Sine: " << setw(16) << orbit.getCus() << " rad Cosine: "
1589  << setw(16) << orbit.getCuc() << " rad" << endl;
1590 
1591  s << endl
1592  << " SV STATUS"
1593  << endl
1594  << endl
1595  << "Health bits: 0x" << setfill('0') << setw(2) << hex << health << dec
1596  << " URA index: " << setfill(' ') << setw(4) << orbit.getURAoe() << endl
1597  << "Code on L2: ";
1598 
1599  switch (codeflags)
1600  {
1601  case 0:
1602  s << "reserved ";
1603  break;
1604 
1605  case 1:
1606  s << " P only ";
1607  break;
1608 
1609  case 2:
1610  s << " C/A only";
1611  break;
1612 
1613  case 3:
1614  s << " P & C/A ";
1615  break;
1616 
1617  default:
1618  break;
1619 
1620  }
1621  if(isFIC)
1622  {
1623  s << " L2 P Nav data: ";
1624  if (L2Pdata!=0)
1625  s << "off";
1626  else
1627  s << "on";
1628  }
1629 
1630  s << endl;
1631  s.flags(oldFlags);
1632 
1633  } // end of SF123::dump()
1634 
1635  ostream& operator<<(ostream& s, const EngEphemeris& eph)
1636  {
1637  try
1638  {
1639  eph.dump(s);
1640  }
1641  catch(gnsstk::Exception& ex)
1642  {
1644  GNSSTK_RETHROW(ex);
1645  }
1646  return s;
1647 
1648  } // end of operator<<
1649 
1650 } // namespace
TimeSystem.hpp
gnsstk::EngEphemeris::getTLMMessage
unsigned getTLMMessage(short subframe) const
Definition: EngEphemeris.cpp:557
gnsstk::EngEphemeris::getTgd
double getTgd() const
Definition: EngEphemeris.cpp:795
YDSTime.hpp
gnsstk::EngEphemeris::getHealth
short getHealth() const
Definition: EngEphemeris.cpp:705
gnsstk::EngEphemeris::dump
void dump(std::ostream &s=std::cout) const
Definition: EngEphemeris.cpp:1438
gnsstk::EngEphemeris::getA
double getA() const
Definition: EngEphemeris.cpp:915
gnsstk::HALFWEEK
const long HALFWEEK
Seconds per half week.
Definition: TimeConstants.hpp:58
gnsstk::EngEphemeris::svClockBias
double svClockBias(const CommonTime &t) const
Definition: EngEphemeris.cpp:547
gnsstk::BrcKeplerOrbit
Definition: BrcKeplerOrbit.hpp:81
gnsstk::EngEphemeris::getFitInterval
short getFitInterval() const
Definition: EngEphemeris.cpp:428
gnsstk::EngEphemeris::setAccuracy
void setAccuracy(double acc)
Definition: EngEphemeris.cpp:417
const
#define const
Definition: getopt.c:43
StringUtils.hpp
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::EngEphemeris::getTracker
short getTracker() const
Definition: EngEphemeris.cpp:629
gnsstk::EngEphemeris::getEcc
double getEcc() const
Definition: EngEphemeris.cpp:895
gnsstk::FULLWEEK
const long FULLWEEK
Seconds per whole week.
Definition: TimeConstants.hpp:60
GPS_URA.hpp
gnsstk::EngEphemeris::EngEphemeris
EngEphemeris() noexcept
Default constructor.
Definition: EngEphemeris.cpp:62
gnsstk::EngEphemeris::getFullWeek
short getFullWeek() const
Definition: EngEphemeris.cpp:665
gnsstk::EngEphemeris::getEpochTime
CommonTime getEpochTime() const
Definition: EngEphemeris.cpp:588
example6.hour
hour
Definition: example6.py:67
gnsstk::EngEphemeris::svClockDrift
double svClockDrift(const CommonTime &t) const
Definition: EngEphemeris.cpp:552
gnsstk::StringUtils::asString
std::string asString(IonexStoreStrategy e)
Convert a IonexStoreStrategy to a whitespace-free string name.
Definition: IonexStoreStrategy.cpp:46
gnsstk::EngEphemeris::getASAlert
short getASAlert(short subframe) const
Definition: EngEphemeris.cpp:654
gnsstk::EngEphemeris::getIDot
double getIDot() const
Definition: EngEphemeris.cpp:965
gnsstk::EngEphemeris::getOmega0
double getOmega0() const
Definition: EngEphemeris.cpp:925
gnsstk::EngEphemeris::getAccFlag
short getAccFlag() const
Definition: EngEphemeris.cpp:695
gnsstk::Xvt::v
Triple v
satellite velocity in ECEF Cartesian, meters/second
Definition: Xvt.hpp:152
gnsstk::EngEphemeris::getCrc
double getCrc() const
Definition: EngEphemeris.cpp:835
gnsstk::EngEphemeris::getAhalf
double getAhalf() const
Definition: EngEphemeris.cpp:905
GNSSconstants.hpp
gnsstk::Exception::addLocation
Exception & addLocation(const ExceptionLocation &location)
Definition: Exception.cpp:108
gnsstk::EngEphemeris::getHOWTime
double getHOWTime(short subframe) const
Definition: EngEphemeris.cpp:639
gnsstk::EngEphemeris::getAf1
double getAf1() const
Definition: EngEphemeris.cpp:775
gnsstk::EngEphemeris::getEphemerisEpoch
CommonTime getEphemerisEpoch() const
Definition: EngEphemeris.cpp:593
gnsstk
For Sinex::InputHistory.
Definition: BasicFramework.cpp:50
gnsstk::EngEphemeris::addIncompleteSF1Thru3
bool addIncompleteSF1Thru3(const uint32_t sf1[8], const uint32_t sf2[8], const uint32_t sf3[8], const long sf1TransmitSOW, const int gpsWeek, const short PRN, const short track)
Definition: EngEphemeris.cpp:201
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::EngEphemeris::svXvt
Xvt svXvt(const CommonTime &t) const
Definition: EngEphemeris.cpp:525
gnsstk::WeekSecond::getDayOfWeek
virtual unsigned int getDayOfWeek() const
Force this interface on this classes descendants.
Definition: WeekSecond.hpp:134
gnsstk::EngEphemeris::getAODO
long getAODO() const
Definition: EngEphemeris.cpp:745
gnsstk::Exception
Definition: Exception.hpp:151
gnsstk::EngEphemeris::getCis
double getCis() const
Definition: EngEphemeris.cpp:825
gnsstk::ObservationType::NavMsg
@ NavMsg
Navigation Message data.
gnsstk::EngEphemeris
Definition: EngEphemeris.hpp:86
gnsstk::Xvt::x
Triple x
Sat position ECEF Cartesian (X,Y,Z) meters.
Definition: Xvt.hpp:151
gnsstk::EngEphemeris::addSubframeNoParity
bool addSubframeNoParity(const uint32_t subframe[10], const int gpsWeek, const short PRN, const short track)
Definition: EngEphemeris.cpp:181
gnsstk::EngEphemeris::unifiedConvert
bool unifiedConvert(const int gpsWeek, const short PRN, const short track)
Definition: EngEphemeris.cpp:254
gnsstk::EngEphemeris::getL2Pdata
short getL2Pdata() const
Definition: EngEphemeris.cpp:715
gnsstk::EngEphemeris::getAf2
double getAf2() const
Definition: EngEphemeris.cpp:785
gnsstk::bds::sf2
@ sf2
Definition: BDSD1Bits.hpp:192
gnsstk::EngEphemeris::getDn
double getDn() const
Definition: EngEphemeris.cpp:885
gnsstk::EngEphemeris::getIODC
short getIODC() const
Definition: EngEphemeris.cpp:725
gnsstk::ObsID
Definition: ObsID.hpp:82
EngEphemeris.hpp
gnsstk::operator<<
std::ostream & operator<<(std::ostream &s, const ObsEpoch &oe) noexcept
Definition: ObsEpochMap.cpp:54
gnsstk::EngEphemeris::getToc
double getToc() const
Definition: EngEphemeris.cpp:755
gnsstk::CommonTime
Definition: CommonTime.hpp:84
gnsstk::WeekSecond::sow
double sow
Definition: WeekSecond.hpp:155
gnsstk::Xvt::clkdrift
double clkdrift
satellite clock drift in seconds/second
Definition: Xvt.hpp:154
gnsstk::EngEphemeris::getClock
BrcClockCorrection getClock() const
Definition: EngEphemeris.cpp:608
gnsstk::min
T min(const SparseMatrix< T > &SM)
Maximum element - return 0 if empty.
Definition: SparseMatrix.hpp:858
gnsstk::EngEphemeris::setSF1
EngEphemeris & setSF1(unsigned tlm, double how, short asalert, short fullweek, short cflags, short acc, short svhealth, short iodc, short l2pdata, double tgd, double toc, double Af2, double Af1, double Af0, short Tracker, short prn)
Definition: EngEphemeris.cpp:1108
gnsstk::EngEphemeris::loadData
EngEphemeris & loadData(const std::string satSysArg, const unsigned short tlm[3], const long how[3], const short asalert[3], const short Tracker, const short prn, const short fullweek, const short cflags, const short acc, const short svhealth, const short iodc, const short l2pdata, const long Aodo, const double tgd, const double toc, const double Af2, const double Af1, const double Af0, const short iode, const double crs, const double Dn, const double m0, const double cuc, const double Ecc, const double cus, const double ahalf, const double toe, const short fitInt, const double cic, const double Omega0, const double cis, const double I0, const double crc, const double W, const double OmegaDot, const double IDot)
Definition: EngEphemeris.cpp:1019
gnsstk::getLegacyFitInterval
short getLegacyFitInterval(const short iodc, const short fiti)
Definition: GNSSconstants.hpp:111
gnsstk::EngEphemeris::getTransmitTime
CommonTime getTransmitTime() const
Definition: EngEphemeris.cpp:568
gnsstk::EngEphemeris::getPRNID
short getPRNID() const
Definition: EngEphemeris.cpp:619
arg
double arg
Definition: IERS1996UT1mUTCData.hpp:48
gnsstk::Xvt
Definition: Xvt.hpp:60
CivilTime.hpp
gnsstk::Week::week
int week
Full week number.
Definition: Week.hpp:267
gnsstk::EngEphemeris::setSF3
EngEphemeris & setSF3(unsigned tlm, double how, short asalert, double cic, double Omega0, double cis, double I0, double crc, double W, double OmegaDot, double IDot)
Definition: EngEphemeris.cpp:1247
GNSSTK_RETHROW
#define GNSSTK_RETHROW(exc)
Definition: Exception.hpp:369
GPSEllipsoid.hpp
gnsstk::EngEphemeris::getIODE
short getIODE() const
Definition: EngEphemeris.cpp:735
gnsstk::EngEphemeris::getCuc
double getCuc() const
Definition: EngEphemeris.cpp:845
gnsstk::CivilTime
Definition: CivilTime.hpp:55
gnsstk::EngEphemeris::getCrs
double getCrs() const
Definition: EngEphemeris.cpp:815
GPSWeekSecond.hpp
gnsstk::EngEphemeris::getW
double getW() const
Definition: EngEphemeris.cpp:945
gnsstk::bds::sf1
@ sf1
Definition: BDSD1Bits.hpp:191
gnsstk::printTime
std::string printTime(const CommonTime &t, const std::string &fmt)
Definition: TimeString.cpp:64
gnsstk::EngEphemeris::setFIC
void setFIC(const bool arg)
Definition: EngEphemeris.cpp:1334
gnsstk::TimeSystem::GPS
@ GPS
GPS system time.
std
Definition: Angle.hpp:142
gnsstk::timeDisplay
static void timeDisplay(ostream &os, const CommonTime &t)
Definition: BrcClockCorrection.cpp:352
gnsstk::EngEphemeris::dumpTerse
void dumpTerse(std::ostream &s=std::cout) const
Definition: EngEphemeris.cpp:1398
gnsstk::CarrierBand::Undefined
@ Undefined
Code is known to be undefined (as opposed to unknown)
gnsstk::BrcClockCorrection
Definition: BrcClockCorrection.hpp:76
gnsstk::EngEphemeris::getM0
double getM0() const
Definition: EngEphemeris.cpp:875
gnsstk::EngEphemeris::isDataSet
bool isDataSet() const
Definition: EngEphemeris.cpp:408
gnsstk::EngEphemeris::setSF2
EngEphemeris & setSF2(unsigned tlm, double how, short asalert, short iode, double crs, double Dn, double m0, double cuc, double Ecc, double cus, double ahalf, double toe, short fitInt)
Definition: EngEphemeris.cpp:1161
GNSSTK_THROW
#define GNSSTK_THROW(exc)
Definition: Exception.hpp:366
gnsstk::Xvt::clkbias
double clkbias
Sat clock correction in seconds.
Definition: Xvt.hpp:153
gnsstk::EngEphemeris::getAccuracy
double getAccuracy() const
Definition: EngEphemeris.cpp:685
gnsstk::EngEphemeris::isValid
bool isValid() const noexcept
Definition: EngEphemeris.cpp:377
MathBase.hpp
gnsstk::EngEphemeris::getCic
double getCic() const
Definition: EngEphemeris.cpp:855
FILE_LOCATION
#define FILE_LOCATION
Definition: Exception.hpp:352
gnsstk::EngEphemeris::getAf0
double getAf0() const
Definition: EngEphemeris.cpp:765
gnsstk::EngEphemeris::operator==
bool operator==(const EngEphemeris &right) const noexcept
Definition: EngEphemeris.cpp:101
gnsstk::EngEphemeris::isData
bool isData(short subframe) const
Definition: EngEphemeris.cpp:396
gnsstk::EngEphemeris::getCus
double getCus() const
Definition: EngEphemeris.cpp:805
gnsstk::EngEphemeris::getCodeFlags
short getCodeFlags() const
Definition: EngEphemeris.cpp:675
gnsstk::EngEphemeris::getI0
double getI0() const
Definition: EngEphemeris.cpp:935
gnsstk::EngEphemeris::getTot
long getTot() const
Definition: EngEphemeris.cpp:985
gnsstk::EngEphemeris::getToe
double getToe() const
Definition: EngEphemeris.cpp:865
gnsstk::EngEphemeris::getFitInt
short getFitInt() const
Definition: EngEphemeris.cpp:975
gnsstk::TrackingCode::Undefined
@ Undefined
Code is known to be undefined (as opposed to unknown)
gnsstk::bds::sf3
@ sf3
Definition: BDSD1Bits.hpp:193
gnsstk::EngEphemeris::getOmegaDot
double getOmegaDot() const
Definition: EngEphemeris.cpp:955
TimeString.hpp
gnsstk::EngEphemeris::getOrbit
BrcKeplerOrbit getOrbit() const
Definition: EngEphemeris.cpp:598
gnsstk::EngEphemeris::addSubframe
bool addSubframe(const uint32_t subframe[10], const int gpsWeek, const short PRN, const short track)
Definition: EngEphemeris.cpp:146
gnsstk::EngEphemeris::svRelativity
double svRelativity(const CommonTime &t) const
Definition: EngEphemeris.cpp:542


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