2019-08-12 17:15:25 -06:00
# include <string>
2019-08-29 04:45:37 -06:00
# include <stdio.h>
2019-08-12 17:15:25 -06:00
# include <iostream>
# include <arpa/inet.h>
# include "fmt/format.h"
# include "fmt/printf.h"
# include <fstream>
# include <map>
# include <bitset>
# include <vector>
# include <thread>
# include <signal.h>
# include <time.h>
# include "ubx.hh"
# include "bits.hh"
# include "minivec.hh"
# include "navmon.pb.h"
# include "ephemeris.hh"
# include "gps.hh"
2019-08-26 07:58:01 -06:00
# include "glonass.hh"
# include "beidou.hh"
2019-08-29 04:45:37 -06:00
# include "galileo.hh"
2019-08-30 17:08:56 -06:00
# include "navmon.hh"
2019-09-02 08:13:49 -06:00
# include "tle.hh"
2019-08-26 07:58:01 -06:00
2019-08-12 17:15:25 -06:00
# include <unistd.h>
using namespace std ;
2019-09-16 06:33:06 -06:00
Point g_ourpos ;
2019-08-29 08:46:03 -06:00
string beidouHealth ( int in )
{
string ret ;
if ( in = = 256 ) {
return " NO CLOCK " ;
}
if ( in = = 511 ) {
return " NO SAT " ;
}
if ( in & ( 1 < < 7 ) )
ret + = " B1I abnormal " ;
if ( in & ( 1 < < 6 ) )
ret + = " B2I abnormal " ;
if ( in & ( 1 < < 5 ) )
ret + = " B3I abnormal " ;
if ( in & ( 1 < < 1 ) )
ret + = " navigation abnormal " ;
if ( ret . empty ( ) )
return " OK " ;
return ret ;
}
2019-09-09 12:49:31 -06:00
double utcFromGPS ( int wn , double tow )
{
return ( 315964800 + wn * 7 * 86400 + tow - 18 ) ;
}
2019-09-22 09:48:02 -06:00
static double utcFromGST ( int wn , double tow )
{
return ( 935280000.0 + wn * 7 * 86400 + tow - 18 ) ;
}
// GALILEO ONLY!!
template < typename T >
void doOrbitDump ( int gnss , int sv , int wn , const T & oldEph , const T & newEph , int time_start , int time_end )
{
ofstream orbitcsv ( " orbit. " + to_string ( gnss ) + " . " + to_string ( sv ) + " . " + to_string ( oldEph . iodnav ) + " - " + to_string ( newEph . iodnav ) + " .csv " ) ;
orbitcsv < < " timestamp x y z oldx oldy oldz \n " ;
orbitcsv < < fixed ;
for ( int t = time_start ; t < time_end ; t + = 30 ) {
Point p , oldp ;
getCoordinates ( t , newEph , & p ) ;
getCoordinates ( t , oldEph , & oldp ) ;
time_t posix = utcFromGST ( wn , t ) ;
orbitcsv < < posix < < " "
< < p . x < < " " < < p . y < < " " < < p . z < < " "
< < oldp . x < < " " < < oldp . y < < " " < < oldp . z < < " \n " ;
}
}
2019-09-09 12:49:31 -06:00
2019-08-12 17:15:25 -06:00
int main ( int argc , char * * argv )
2019-09-02 08:13:49 -06:00
try
2019-08-12 17:15:25 -06:00
{
2019-09-02 08:13:49 -06:00
TLERepo tles ;
2019-09-03 12:05:40 -06:00
// tles.parseFile("active.txt");
tles . parseFile ( " galileo.txt " ) ;
2019-09-02 08:13:49 -06:00
tles . parseFile ( " glo-ops.txt " ) ;
tles . parseFile ( " gps-ops.txt " ) ;
2019-09-03 12:05:40 -06:00
tles . parseFile ( " beidou.txt " ) ;
2019-09-23 06:23:03 -06:00
bool skipGPS { false } ;
bool skipBeidou { false } ;
2019-09-09 12:49:31 -06:00
bool skipGalileo { false } ;
2019-09-23 06:23:03 -06:00
bool skipGlonass { false } ;
2019-09-09 12:49:31 -06:00
2019-09-03 12:05:40 -06:00
ofstream almanac ( " almanac.txt " ) ;
ofstream iodstream ( " iodstream.csv " ) ;
iodstream < < " timestamp gnssid sv iodnav t0e age " < < endl ;
2019-09-09 12:49:31 -06:00
ofstream csv ( " delta.csv " ) ;
csv < < " timestamp gnssid sv tow tle_distance alma_distance utc_dist x y z vx vy vz rad inclination e iod " < < endl ;
2019-09-02 08:13:49 -06:00
2019-08-12 17:15:25 -06:00
for ( ; ; ) {
char bert [ 4 ] ;
2019-08-30 17:08:56 -06:00
int res = readn2 ( 0 , bert , 4 ) ;
2019-08-29 08:46:03 -06:00
if ( res ! = 4 ) {
cerr < < " EOF, res = " < < res < < endl ;
2019-08-12 17:15:25 -06:00
break ;
}
2019-08-29 08:46:03 -06:00
if ( bert [ 0 ] ! = ' b ' | | bert [ 1 ] ! = ' e ' | | bert [ 2 ] ! = ' r ' | | bert [ 3 ] ! = ' t ' ) {
cerr < < " Bad magic " < < endl ;
}
2019-08-12 17:15:25 -06:00
uint16_t len ;
2019-08-30 17:08:56 -06:00
if ( readn2 ( 0 , & len , 2 ) ! = 2 )
2019-08-12 17:15:25 -06:00
break ;
len = htons ( len ) ;
char buffer [ len ] ;
2019-08-30 17:08:56 -06:00
if ( readn2 ( 0 , buffer , len ) ! = len )
2019-08-12 17:15:25 -06:00
break ;
NavMonMessage nmm ;
nmm . ParseFromString ( string ( buffer , len ) ) ;
2019-08-30 09:56:41 -06:00
2019-09-02 08:13:49 -06:00
// if(nmm.type() == NavMonMessage::ReceptionDataType)
// continue;
2019-08-30 09:56:41 -06:00
2019-08-12 17:15:25 -06:00
cout < < humanTime ( nmm . localutcseconds ( ) ) < < " " < < nmm . localutcnanoseconds ( ) < < " " ;
cout < < " src " < < nmm . sourceid ( ) < < " " ;
if ( nmm . type ( ) = = NavMonMessage : : ReceptionDataType ) {
2019-09-16 06:33:06 -06:00
cout < < " receptiondata for " < < nmm . rd ( ) . gnssid ( ) < < " , " < < nmm . rd ( ) . gnsssv ( ) < < " , " < < ( nmm . rd ( ) . has_sigid ( ) ? nmm . rd ( ) . sigid ( ) : 0 ) < < " db " < < nmm . rd ( ) . db ( ) < < " ele " < < nmm . rd ( ) . el ( ) < < " azi " < < nmm . rd ( ) . azi ( ) < < " prRes " < < nmm . rd ( ) . prres ( ) < < endl ;
2019-08-12 17:15:25 -06:00
}
else if ( nmm . type ( ) = = NavMonMessage : : GalileoInavType ) {
2019-09-09 12:49:31 -06:00
if ( skipGalileo )
continue ;
2019-08-12 17:15:25 -06:00
basic_string < uint8_t > inav ( ( uint8_t * ) nmm . gi ( ) . contents ( ) . c_str ( ) , nmm . gi ( ) . contents ( ) . size ( ) ) ;
2019-08-29 04:45:37 -06:00
static map < int , GalileoMessage > gms ;
2019-09-02 08:13:49 -06:00
static map < pair < int , int > , GalileoMessage > gmwtypes ;
2019-08-30 09:56:41 -06:00
static map < int , GalileoMessage > oldgm4s ;
2019-09-02 08:13:49 -06:00
int sv = nmm . gi ( ) . gnsssv ( ) ;
GalileoMessage & gm = gms [ sv ] ;
2019-08-29 04:45:37 -06:00
int wtype = gm . parse ( inav ) ;
2019-09-05 01:33:50 -06:00
2019-09-02 08:13:49 -06:00
gm . tow = nmm . gi ( ) . gnsstow ( ) ;
gmwtypes [ { nmm . gi ( ) . gnsssv ( ) , wtype } ] = gm ;
2019-09-22 08:42:23 -06:00
static map < int , GalileoMessage > oldEph ;
2019-09-16 06:33:06 -06:00
cout < < " gal inav for " < < nmm . gi ( ) . gnssid ( ) < < " , " < < nmm . gi ( ) . gnsssv ( ) < < " , " < < nmm . gi ( ) . sigid ( ) < < " tow " < < nmm . gi ( ) . gnsstow ( ) < < " wtype " < < wtype < < " " ;
2019-08-30 09:56:41 -06:00
static uint32_t tow ;
2019-08-29 04:45:37 -06:00
if ( wtype = = 4 ) {
// 2^-34 2^-46
2019-09-22 08:42:23 -06:00
cout < < " iodnav " < < gm . iodnav < < " af0 " < < gm . af0 < < " af1 " < < gm . af1 < < " , scaled: " < < ldexp ( 1.0 * gm . af0 , 19 - 34 ) < < " , " < < ldexp ( 1.0 * gm . af1 , 38 - 46 ) ;
2019-08-30 09:56:41 -06:00
if ( tow & & oldgm4s . count ( nmm . gi ( ) . gnsssv ( ) ) & & oldgm4s [ nmm . gi ( ) . gnsssv ( ) ] . iodnav ! = gm . iodnav ) {
auto & oldgm4 = oldgm4s [ nmm . gi ( ) . gnsssv ( ) ] ;
auto oldOffset = oldgm4 . getAtomicOffset ( tow ) ;
auto newOffset = gm . getAtomicOffset ( tow ) ;
cout < < " Timejump: " < < oldOffset . first - newOffset . first < < " after " < < ( gm . getT0c ( ) - oldgm4 . getT0c ( ) ) < < " seconds " ;
}
2019-09-02 08:13:49 -06:00
2019-08-30 09:56:41 -06:00
oldgm4s [ nmm . gi ( ) . gnsssv ( ) ] = gm ;
2019-09-22 09:48:02 -06:00
int sv = nmm . gi ( ) . gnsssv ( ) ;
if ( gmwtypes [ { sv , 1 } ] . iodnav = = gmwtypes [ { sv , 2 } ] . iodnav & &
gmwtypes [ { sv , 2 } ] . iodnav = = gmwtypes [ { sv , 3 } ] . iodnav & &
gmwtypes [ { sv , 3 } ] . iodnav = = gmwtypes [ { sv , 4 } ] . iodnav ) {
cout < < " have complete ephemeris at " < < gm . iodnav ;
if ( ! oldEph [ sv ] . sqrtA )
oldEph [ sv ] = gm ;
else if ( oldEph [ sv ] . iodnav ! = gm . iodnav ) {
cout < < " disco! " < < oldEph [ sv ] . iodnav < < " - > " < < gm . iodnav < < " , " < < ( gm . getT0e ( ) - oldEph [ sv ] . getT0e ( ) ) / 3600.0 < < " hours-jump insta-age " < < ephAge ( gm . tow , gm . getT0e ( ) ) / 3600.0 < < " hours " ;
Point oldPoint , newPoint ;
getCoordinates ( gm . tow , oldEph [ sv ] , & oldPoint ) ;
getCoordinates ( gm . tow , gm , & newPoint ) ;
Vector jump ( oldPoint , newPoint ) ;
cout < < " distance " < < jump . length ( ) < < " ( " < < jump . x < < " , " < < jump . y < < " , " < < jump . z < < " ) " ;
auto oldAtomic = oldEph [ sv ] . getAtomicOffset ( gm . tow ) ;
auto newAtomic = gm . getAtomicOffset ( gm . tow ) ;
cout < < " clock-jump " < < oldAtomic . first - newAtomic . first < < " ns " ;
doOrbitDump ( 2 , sv , gm . wn , oldEph [ sv ] , gm , gm . tow - 3 * 3600 , gm . tow + 3 * 3600 ) ;
oldEph [ sv ] = gm ;
}
}
2019-08-29 04:45:37 -06:00
}
2019-09-18 06:17:41 -06:00
if ( wtype = = 1 ) {
cout < < " iodnav " < < gm . iodnav < < " t0e " < < gm . t0e * 60 < < " " < < ephAge ( gm . t0e * 60 , gm . tow ) ;
}
if ( wtype = = 2 | | wtype = = 3 ) {
cout < < " iodnav " < < gm . iodnav ;
}
2019-09-03 12:05:40 -06:00
if ( wtype = = 1 | | wtype = = 2 | | wtype = = 3 ) {
iodstream < < nmm . localutcseconds ( ) < < " " < < nmm . gi ( ) . gnssid ( ) < < " " < < nmm . gi ( ) . gnsssv ( ) < < " " < < gm . iodnav < < " " < < gm . t0e * 60 < < " " < < ephAge ( gm . t0e * 60 , gm . tow ) ;
if ( wtype = = 3 )
iodstream < < endl ;
else
iodstream < < " \n " ;
}
2019-08-29 04:45:37 -06:00
if ( wtype = = 0 | | wtype = = 5 | | wtype = = 6 )
tow = gm . tow ;
2019-09-03 12:05:40 -06:00
// if(wtype < 7)
// gm = GalileoMessage{};
2019-08-29 04:45:37 -06:00
2019-09-02 08:13:49 -06:00
if ( wtype > = 7 & & wtype < = 10 )
cout < < " ioda " < < gm . iodalmanac ;
2019-08-29 04:45:37 -06:00
// af0 af1 scaling in almanac: 2^-19, 2^2^-38 plus "truncated"
if ( wtype = = 7 ) {
2019-09-05 01:33:50 -06:00
// this is possible because all the ephemeris stuff is in 7
2019-09-02 08:13:49 -06:00
cout < < " t0a " < < gm . t0almanac < < " , alma sv1 " < < gm . alma1 . svid < < " , t0a age: " < < ephAge ( gm . t0almanac * 600 , tow ) < < " " ;
if ( gm . alma1 . svid ) {
Point satpos ;
2019-09-16 06:33:06 -06:00
getCoordinates ( gm . tow , gm . alma1 , & satpos ) ;
2019-09-02 08:13:49 -06:00
cout < < " ( " < < satpos . x / 1000 < < " , " < < satpos . y / 1000 < < " , " < < satpos . z / 1000 < < " ) " ;
auto match = tles . getBestMatch ( nmm . localutcseconds ( ) , satpos . x , satpos . y , satpos . z ) ;
cout < < " best-tle-match " < < match . name < < " distance " < < match . distance / 1000 < < " km " ;
cout < < " tle-e " < < match . e < < " eph-e " < < gm . alma1 . getE ( ) < < " tle-ran " < < match . ran ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
2019-09-16 06:33:06 -06:00
cout < < " ele " < < getElevationDeg ( satpos , g_ourpos ) < < " azi " < < getAzimuthDeg ( satpos , g_ourpos ) ;
2019-09-02 08:13:49 -06:00
}
2019-08-29 04:45:37 -06:00
}
2019-09-02 08:13:49 -06:00
else if ( wtype = = 8 & & gm . tow - gmwtypes [ { sv , 7 } ] . tow < 5 & & gmwtypes [ { sv , 7 } ] . alma1 . svid & & gm . iodalmanac = = gmwtypes [ { sv , 7 } ] . iodalmanac ) {
2019-09-05 01:33:50 -06:00
// 8 finishes the rest
cout < < " alma1.sv " < < gmwtypes [ { sv , 7 } ] . alma1 . svid < < " af0 " < < gm . alma1 . af0 < < " af1 " < < gm . alma1 . af1 < < " e5bhs " < < gm . alma1 . e5bhs < < " e1bhs " < < gm . alma1 . e1bhs < < " sv9 " < < gm . alma2 . svid ;
2019-08-29 04:45:37 -06:00
}
2019-09-02 08:13:49 -06:00
else if ( wtype = = 9 & & gm . tow - gmwtypes [ { sv , 8 } ] . tow < 30 & & gm . iodalmanac = = gmwtypes [ { sv , 8 } ] . iodalmanac ) {
if ( gmwtypes [ { sv , 8 } ] . alma2 . svid )
2019-09-05 01:33:50 -06:00
cout < < " alma2.sv " < < gmwtypes [ { sv , 8 } ] . alma2 . svid < < " af0 " < < gm . alma2 . af0 < < " af1 " < < gm . alma2 . af1 < < " e5bhs " < < gm . alma2 . e5bhs < < " e1bhs " < < gm . alma2 . e1bhs ;
2019-09-02 08:13:49 -06:00
else
cout < < " empty almanac slot " ;
2019-08-29 04:45:37 -06:00
}
2019-09-02 08:13:49 -06:00
else if ( wtype = = 10 & & gm . tow - gmwtypes [ { sv , 9 } ] . tow < 5 & & gmwtypes [ { sv , 9 } ] . alma3 . svid & & gm . iodalmanac = = gmwtypes [ { sv , 9 } ] . iodalmanac ) {
if ( gm . alma3 . e1bhs ! = 0 ) {
cout < < " gm.tow " < < gm . tow < < " gmwtypes.tow " < < gmwtypes [ { sv , 9 } ] . tow < < " " ;
}
cout < < " " < < gmwtypes [ { sv , 9 } ] . alma3 . svid < < " af0 " < < gm . alma3 . af0 < < " af1 " < < gm . alma3 . af1 < < " e5bhs " < < gm . alma3 . e5bhs < < " e1bhs " < < gm . alma3 . e1bhs < < " a0g " < < gm . a0g < < " a1g " < < gm . a1g < < " t0g " < < gm . t0g < < " wn0g " < < gm . wn0g ;
2019-08-29 04:45:37 -06:00
}
2019-09-22 08:42:23 -06:00
2019-09-02 08:13:49 -06:00
cout < < endl ;
2019-08-12 17:15:25 -06:00
}
else if ( nmm . type ( ) = = NavMonMessage : : GPSInavType ) {
2019-09-22 08:42:23 -06:00
if ( skipGPS ) {
cout < < endl ;
2019-09-09 12:49:31 -06:00
continue ;
2019-09-22 08:42:23 -06:00
}
2019-09-09 12:49:31 -06:00
2019-08-12 17:15:25 -06:00
int sv = nmm . gpsi ( ) . gnsssv ( ) ;
2019-08-17 02:30:49 -06:00
auto cond = getCondensedGPSMessage ( std : : basic_string < uint8_t > ( ( uint8_t * ) nmm . gpsi ( ) . contents ( ) . c_str ( ) , nmm . gpsi ( ) . contents ( ) . size ( ) ) ) ;
struct GPSState gs ;
2019-09-09 12:49:31 -06:00
static map < int , GPSState > eph ;
static map < int , GPSAlmanac > almas ;
2019-08-17 02:30:49 -06:00
uint8_t page ;
2019-09-09 12:49:31 -06:00
static int gpswn ;
2019-08-17 02:30:49 -06:00
int frame = parseGPSMessage ( cond , gs , & page ) ;
2019-09-16 06:33:06 -06:00
cout < < " GPS " < < sv < < " @ " < < nmm . gpsi ( ) . sigid ( ) < < " : " < < gs . tow < < " frame " < < frame < < " " ;
2019-08-17 03:05:03 -06:00
if ( frame = = 1 ) {
2019-08-30 09:56:41 -06:00
static map < int , GPSState > oldgs1s ;
2019-09-09 12:49:31 -06:00
gpswn = gs . wn ;
2019-08-30 09:56:41 -06:00
cout < < " gpshealth = " < < ( int ) gs . gpshealth < < " , wn " < < gs . wn < < " t0c " < < gs . t0c ;
if ( auto iter = oldgs1s . find ( sv ) ; iter ! = oldgs1s . end ( ) & & iter - > second . t0c ! = gs . t0c ) {
2019-09-09 12:49:31 -06:00
auto oldOffset = getGPSAtomicOffset ( gs . tow , iter - > second ) ;
auto newOffset = getGPSAtomicOffset ( gs . tow , gs ) ;
2019-08-30 09:56:41 -06:00
cout < < " Timejump: " < < oldOffset . first - newOffset . first < < " after " < < ( getT0c ( gs ) - getT0c ( iter - > second ) ) < < " seconds, old t0c " < < iter - > second . t0c ;
}
oldgs1s [ sv ] = gs ;
2019-08-17 03:05:03 -06:00
}
else if ( frame = = 2 ) {
2019-09-09 12:49:31 -06:00
parseGPSMessage ( cond , eph [ sv ] , & page ) ;
cout < < " t0e = " < < gs . iods . begin ( ) - > second . t0e < < " " < < ephAge ( gs . tow , gs . iods . begin ( ) - > second . t0e ) < < " iod " < < gs . gpsiod ;
}
else if ( frame = = 3 ) {
parseGPSMessage ( cond , eph [ sv ] , & page ) ;
cout < < " iod " < < gs . gpsiod ;
if ( eph [ sv ] . isComplete ( gs . gpsiod ) ) {
Point sat ;
2019-09-16 06:33:06 -06:00
getCoordinates ( gs . tow , eph [ sv ] . iods [ gs . gpsiod ] , & sat ) ;
2019-09-09 12:49:31 -06:00
TLERepo : : Match second ;
auto match = tles . getBestMatch ( utcFromGPS ( gpswn , gs . tow ) , sat . x , sat . y , sat . z , & second ) ;
cout < < " best-tle-match " < < match . name < < " dist " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
cout < < " 2nd-match " < < second . name < < " dist " < < second . distance / 1000 < < " km t0e " < < gs . gpsalma . getT0e ( ) < < " t " < < nmm . localutcseconds ( ) ;
2019-09-16 06:33:06 -06:00
cout < < " ele " < < getElevationDeg ( sat , g_ourpos ) < < " azi " < < getAzimuthDeg ( sat , g_ourpos ) ;
2019-09-09 12:49:31 -06:00
if ( almas . count ( sv ) ) {
Point almapoint ;
2019-09-16 06:33:06 -06:00
getCoordinates ( gs . tow , almas [ sv ] , & almapoint ) ;
2019-09-09 12:49:31 -06:00
cout < < " alma-dist " < < Vector ( sat , almapoint ) . length ( ) ;
Vector speed ;
2019-09-16 06:33:06 -06:00
getSpeed ( gs . tow , eph [ sv ] . iods [ gs . gpsiod ] , & speed ) ;
2019-09-09 12:49:31 -06:00
Point core ;
csv < < nmm . localutcseconds ( ) < < " 0 " < < sv < < " " < < gs . tow < < " " < < match . distance < < " " < < Vector ( sat , almapoint ) . length ( ) < < " " < < utcFromGPS ( gpswn , gs . tow ) - nmm . localutcseconds ( ) < < " " < < sat . x < < " " < < sat . y < < " " < < sat . z < < " " < < speed . x < < " " < < speed . y < < " " < < speed . z < < " " < < Vector ( core , sat ) . length ( ) < < " " < < eph [ sv ] . iods [ gs . gpsiod ] . getI0 ( ) < < " " < < eph [ sv ] . iods [ gs . gpsiod ] . getE ( ) < < " " < < gs . gpsiod < < endl ;
}
}
2019-08-17 02:30:49 -06:00
}
2019-09-09 12:49:31 -06:00
else if ( frame = = 4 ) {
cout < < " page/svid " < < gs . gpsalma . sv ;
if ( ( gs . gpsalma . sv > = 25 & & gs . gpsalma . sv < = 32 ) | | gs . gpsalma . sv = = 57 ) { // see table 20-V of the GPS ICD
cout < < " data-id " < < gs . gpsalma . dataid < < " alma-sv " < < gs . gpsalma . sv < < " t0a = " < < gs . gpsalma . getT0e ( ) < < " health " < < gs . gpsalma . health ;
Point sat ;
2019-09-16 06:33:06 -06:00
getCoordinates ( gs . tow , gs . gpsalma , & sat ) ;
2019-09-09 12:49:31 -06:00
TLERepo : : Match second ;
auto match = tles . getBestMatch ( nmm . localutcseconds ( ) , sat . x , sat . y , sat . z , & second ) ;
cout < < " best-tle-match " < < match . name < < " dist " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
cout < < " 2nd-match " < < second . name < < " dist " < < second . distance / 1000 < < " km t0e " < < gs . gpsalma . getT0e ( ) < < " t " < < nmm . localutcseconds ( ) ;
}
}
else if ( frame = = 5 ) {
if ( gs . gpsalma . sv < = 24 ) {
cout < < " alma-sv " < < gs . gpsalma . sv < < " t0a = " < < gs . gpsalma . getT0e ( ) < < " health " < < gs . gpsalma . health ;
Point sat ;
2019-09-16 06:33:06 -06:00
getCoordinates ( gs . tow , gs . gpsalma , & sat ) ;
2019-09-09 12:49:31 -06:00
TLERepo : : Match second ;
auto match = tles . getBestMatch ( nmm . localutcseconds ( ) , sat . x , sat . y , sat . z , & second ) ;
cout < < " best-tle-match " < < match . name < < " dist " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
cout < < " 2nd-match " < < second . name < < " dist " < < second . distance / 1000 < < " km t0e " < < gs . gpsalma . getT0e ( ) < < " t " < < nmm . localutcseconds ( ) ;
almas [ gs . gpsalma . sv ] = gs . gpsalma ;
}
}
2019-08-17 02:30:49 -06:00
cout < < " \n " ;
2019-08-12 17:15:25 -06:00
}
2019-08-29 08:46:03 -06:00
else if ( nmm . type ( ) = = NavMonMessage : : BeidouInavTypeD1 ) {
2019-09-22 08:42:23 -06:00
if ( skipBeidou ) {
cout < < endl ;
2019-09-09 12:49:31 -06:00
continue ;
2019-09-22 08:42:23 -06:00
}
2019-09-09 12:49:31 -06:00
2019-08-29 08:46:03 -06:00
int sv = nmm . bid1 ( ) . gnsssv ( ) ;
auto cond = getCondensedBeidouMessage ( std : : basic_string < uint8_t > ( ( uint8_t * ) nmm . bid1 ( ) . contents ( ) . c_str ( ) , nmm . bid1 ( ) . contents ( ) . size ( ) ) ) ;
2019-08-26 07:58:01 -06:00
uint8_t pageno ;
2019-09-02 08:13:49 -06:00
static map < int , BeidouMessage > bms ;
auto & bm = bms [ sv ] ;
2019-08-26 07:58:01 -06:00
int fraid = bm . parse ( cond , & pageno ) ;
2019-09-02 08:13:49 -06:00
2019-08-29 04:45:37 -06:00
cout < < " BeiDou " < < sv < < " : " < < bm . sow < < " , FraID " < < fraid ;
2019-08-29 08:46:03 -06:00
if ( fraid = = 1 ) {
2019-08-30 09:56:41 -06:00
static map < int , BeidouMessage > msgs ;
if ( msgs [ sv ] . wn > = 0 & & msgs [ sv ] . t0c ! = bm . t0c ) {
auto oldOffset = msgs [ sv ] . getAtomicOffset ( bm . sow ) ;
auto newOffset = bm . getAtomicOffset ( bm . sow ) ;
cout < < " Timejump: " < < oldOffset . first - newOffset . first < < " after " < < ( bm . getT0c ( ) - msgs [ sv ] . getT0c ( ) ) < < " seconds " ;
}
msgs [ sv ] = bm ;
cout < < " wn " < < bm . wn < < " t0c " < < ( int ) bm . t0c < < " aodc " < < ( int ) bm . aodc < < " aode " < < ( int ) bm . aode < < " sath1 " < < ( int ) bm . sath1 < < " urai " < < ( int ) bm . urai < < " af0 " < < bm . a0 < < " af1 " < < bm . a1 < < " af2 " < < bm . a2 ;
auto offset = bm . getAtomicOffset ( ) ;
2019-09-16 06:33:06 -06:00
cout < < " , " < < offset . first < < " ns " < < ( offset . second * 3600 ) < < " ns/hour " < < ephAge ( bm . sow , bm . t0c * 8 ) ;
2019-09-02 08:13:49 -06:00
}
else if ( fraid = = 3 & & bm . sow ) {
Point sat ;
2019-09-16 06:33:06 -06:00
getCoordinates ( bm . sow , bm , & sat ) ;
2019-09-02 08:13:49 -06:00
TLERepo : : Match second ;
auto match = tles . getBestMatch ( nmm . localutcseconds ( ) , sat . x , sat . y , sat . z , & second ) ;
cout < < " best-tle-match " < < match . name < < " dist " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
cout < < " 2nd-match " < < second . name < < " dist " < < second . distance / 1000 < < " km " ;
2019-08-29 08:46:03 -06:00
}
2019-09-02 10:11:45 -06:00
else if ( ( fraid = = 4 & & 1 < = pageno & & pageno < = 24 ) | |
( fraid = = 5 & & 1 < = pageno & & pageno < = 6 ) | |
( fraid = = 5 & & 11 < = pageno & & pageno < = 23 ) ) {
2019-09-03 12:05:40 -06:00
struct BeidouAlmanacEntry bae ;
if ( processBeidouAlmanac ( bm , bae ) ) {
cout < < " alma-sv " < < bae . sv ;
Point sat ;
2019-09-16 06:33:06 -06:00
getCoordinates ( bm . sow , bae . alma , & sat ) ;
2019-09-03 12:05:40 -06:00
TLERepo : : Match second ;
auto match = tles . getBestMatch ( nmm . localutcseconds ( ) , sat . x , sat . y , sat . z , & second ) ;
cout < < " best-tle-match " < < match . name < < " dist " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
cout < < " 2nd-match " < < second . name < < " dist " < < second . distance / 1000 < < " km t0e " < < bm . alma . getT0e ( ) < < " t " < < nmm . localutcseconds ( ) ;
}
2019-09-02 10:11:45 -06:00
else
2019-09-03 12:05:40 -06:00
cout < < " no valid alma " ;
2019-08-29 08:46:03 -06:00
}
2019-09-03 12:05:40 -06:00
else if ( bm . fraid = = 5 & & pageno = = 7 ) {
2019-08-29 08:46:03 -06:00
for ( int n = 0 ; n < 19 ; + + n )
cout < < " hea " < < ( 1 + n ) < < " " < < getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) < < " ( " < < beidouHealth ( getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) ) < < " ) " ;
}
2019-09-03 12:05:40 -06:00
else if ( bm . fraid = = 5 & & pageno = = 8 ) {
2019-08-29 08:46:03 -06:00
for ( int n = 0 ; n < 10 ; + + n )
cout < < " hea " < < ( 20 + n ) < < " " < < getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) < < " ( " < < beidouHealth ( getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) ) < < " ) " ;
cout < < " WNa " < < getbitu ( & cond [ 0 ] , beidouBitconv ( 190 ) , 8 ) < < " t0a " < < getbitu ( & cond [ 0 ] , beidouBitconv ( 198 ) , 8 ) ;
}
2019-09-03 12:05:40 -06:00
else if ( bm . fraid = = 5 & & pageno = = 24 ) {
2019-08-29 08:46:03 -06:00
int AmID = getbitu ( & cond [ 0 ] , beidouBitconv ( 216 ) , 2 ) ;
cout < < " AmID " < < AmID ;
for ( int n = 0 ; n < 14 ; + + n )
cout < < " hea " < < ( 31 + n ) < < " ( " < < getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) < < " " < < beidouHealth ( getbitu ( & cond [ 0 ] , beidouBitconv ( 51 + n * 9 ) , 9 ) ) < < " ) " ;
}
2019-08-29 04:45:37 -06:00
cout < < endl ;
2019-08-29 08:46:03 -06:00
}
else if ( nmm . type ( ) = = NavMonMessage : : BeidouInavTypeD2 ) {
int sv = nmm . bid2 ( ) . gnsssv ( ) ;
auto cond = getCondensedBeidouMessage ( std : : basic_string < uint8_t > ( ( uint8_t * ) nmm . bid2 ( ) . contents ( ) . c_str ( ) , nmm . bid2 ( ) . contents ( ) . size ( ) ) ) ;
BeidouMessage bm ;
uint8_t pageno ;
int fraid = bm . parse ( cond , & pageno ) ;
2019-08-12 17:15:25 -06:00
2019-08-29 08:46:03 -06:00
cout < < " BeiDou " < < sv < < " D2: " < < bm . sow < < " , FraID " < < fraid < < endl ;
2019-08-26 07:58:01 -06:00
}
2019-08-30 09:56:41 -06:00
else if ( nmm . type ( ) = = NavMonMessage : : GlonassInavType ) {
2019-09-22 08:42:23 -06:00
if ( skipGlonass ) {
cout < < endl ;
2019-09-09 12:49:31 -06:00
continue ;
2019-09-22 08:42:23 -06:00
}
2019-09-09 12:49:31 -06:00
2019-08-30 17:08:56 -06:00
static map < int , GlonassMessage > gms ;
auto & gm = gms [ nmm . gloi ( ) . gnsssv ( ) ] ;
2019-08-30 09:56:41 -06:00
int strno = gm . parse ( std : : basic_string < uint8_t > ( ( uint8_t * ) nmm . gloi ( ) . contents ( ) . c_str ( ) , nmm . gloi ( ) . contents ( ) . size ( ) ) ) ;
2019-09-02 08:13:49 -06:00
cout < < " Glonass R " < < nmm . gloi ( ) . gnsssv ( ) < < " @ " < < ( ( int ) nmm . gloi ( ) . freq ( ) - 7 ) < < " strno " < < strno ;
2019-08-30 17:08:56 -06:00
if ( strno = = 1 ) {
2019-08-30 09:56:41 -06:00
cout < < " , hour " < < ( int ) gm . hour < < " minute " < < ( int ) gm . minute < < " seconds " < < ( int ) gm . seconds ;
2019-08-30 17:08:56 -06:00
// start of period is 1st of January 1996 + (n4-1)*4, 03:00 UTC
time_t glotime = gm . getGloTime ( ) ;
2019-09-09 12:49:31 -06:00
cout < < " 'wn' " < < glotime / ( 7 * 86400 ) < < " 'tow' " < < ( glotime % ( 7 * 86400 ) ) < < " x " < < gm . getX ( ) / 1000.0 ;
2019-08-30 17:08:56 -06:00
}
2019-09-09 12:49:31 -06:00
else if ( strno = = 2 )
cout < < " Tb " < < ( int ) gm . Tb < < " Bn " < < ( int ) gm . Bn < < " y " < < gm . getY ( ) / 1000.0 ;
else if ( strno = = 3 )
cout < < " l_n " < < ( int ) gm . l_n < < " z " < < gm . getZ ( ) / 1000.0 ;
2019-09-02 08:13:49 -06:00
else if ( strno = = 4 ) {
2019-08-30 09:56:41 -06:00
cout < < " , taun " < < gm . taun < < " NT " < < gm . NT < < " FT " < < ( int ) gm . FT < < " En " < < ( int ) gm . En ;
2019-09-02 08:13:49 -06:00
if ( gm . x & & gm . y & & gm . z ) {
auto longlat = getLongLat ( gm . getX ( ) , gm . getY ( ) , gm . getZ ( ) ) ;
cout < < " long " < < 180 * longlat . first / M_PI < < " lat " < < 180 * longlat . second / M_PI < < " rad " < < gm . getRadius ( ) ;
cout < < " Tb " < < ( int ) gm . Tb < < " H " < < ( ( gm . Tb / 4.0 ) - 3 ) < < " UTC ( " < < gm . getX ( ) / 1000 < < " , " < < gm . getY ( ) / 1000 < < " , " < < gm . getZ ( ) / 1000 < < " ) -> " ;
cout < < " ( " < < gm . getdX ( ) / 1000 < < " , " < < gm . getdY ( ) / 1000 < < " , " < < gm . getdZ ( ) / 1000 < < " ) " ;
2019-09-09 12:49:31 -06:00
auto match = tles . getBestMatch ( getGlonassT0e ( nmm . localutcseconds ( ) , gm . Tb ) , gm . getX ( ) , gm . getY ( ) , gm . getZ ( ) ) ;
2019-09-02 08:13:49 -06:00
cout < < " best-tle-match " < < match . name < < " distance " < < match . distance / 1000 < < " km " ;
cout < < " norad " < < match . norad < < " int-desig " < < match . internat ;
}
}
2019-08-30 17:08:56 -06:00
else if ( strno = = 5 )
cout < < " , n4 " < < ( int ) gm . n4 < < " l_n " < < gm . l_n ;
2019-09-02 08:13:49 -06:00
else if ( strno = = 6 | | strno = = 8 | | strno = = 10 | | strno = = 12 | | strno = = 14 ) {
2019-09-03 12:05:40 -06:00
cout < < " nA " < < gm . nA < < " CnA " < < gm . CnA < < " LambdaNaDeg " < < gm . getLambdaNaDeg ( ) < < " e " < < gm . getE ( ) < < " i0 " < < 180.0 * gm . getI0 ( ) / M_PI ;
}
else if ( strno = = 7 | | strno = = 9 | | strno = = 11 | | strno = = 13 | | strno = = 15 ) {
cout < < " l_n " < < gm . l_n < < " Tlambdana " < < gm . gettLambdaNa ( ) < < " Hna " < < ( int ) gm . hna ;
2019-09-02 08:13:49 -06:00
}
2019-08-30 09:56:41 -06:00
cout < < endl ;
}
2019-08-12 17:15:25 -06:00
else if ( nmm . type ( ) = = NavMonMessage : : ObserverPositionType ) {
2019-09-02 08:13:49 -06:00
auto lonlat = getLongLat ( nmm . op ( ) . x ( ) , nmm . op ( ) . y ( ) , nmm . op ( ) . z ( ) ) ;
2019-09-16 06:33:06 -06:00
cout < < std : : fixed < < " ECEF " < < nmm . op ( ) . x ( ) < < " , " < < nmm . op ( ) . y ( ) < < " , " < < nmm . op ( ) . z ( ) < < " lon " < < 180 * lonlat . first / M_PI < < " lat " < <
180 * lonlat . second / M_PI < < " acc " < < nmm . op ( ) . acc ( ) < < " m " < < endl ;
g_ourpos = Point ( nmm . op ( ) . x ( ) , nmm . op ( ) . y ( ) , nmm . op ( ) . z ( ) ) ;
2019-08-12 17:15:25 -06:00
}
else if ( nmm . type ( ) = = NavMonMessage : : RFDataType ) {
2019-09-16 06:33:06 -06:00
cout < < " RFdata for " < < nmm . rfd ( ) . gnssid ( ) < < " , " < < nmm . rfd ( ) . gnsssv ( ) < < " , " < < ( nmm . rfd ( ) . has_sigid ( ) ? nmm . rfd ( ) . sigid ( ) : 0 ) < < endl ;
2019-08-12 17:15:25 -06:00
}
else {
cout < < " Unknown type " < < ( int ) nmm . type ( ) < < endl ;
}
}
}
2019-09-02 08:13:49 -06:00
catch ( EofException & ee )
{ }