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"
2019-10-01 07:03:30 -06:00
# include "CLI/CLI.hpp"
2019-08-12 17:15:25 -06:00
# 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-10-01 07:03:30 -06:00
# include "sp3.hh"
2020-01-09 06:33:02 -07:00
# include "ubx.hh"
2019-08-12 17:15:25 -06:00
# include <unistd.h>
2020-01-21 11:13:29 -07:00
# include "githash.h"
# include "version.hh"
2020-02-04 13:25:20 -07:00
# include "rinex.hh"
2020-01-21 11:13:29 -07:00
static char program [ ] = " navdump " ;
2019-08-12 17:15:25 -06:00
using namespace std ;
2020-01-21 11:13:29 -07:00
extern const char * g_gitHash ;
2019-09-16 06:33:06 -06:00
Point g_ourpos ;
2019-10-01 07:03:30 -06:00
vector < SP3Entry > g_sp3s ;
bool sp3Order ( const SP3Entry & a , const SP3Entry & b )
{
return tie ( a . gnss , a . sv , a . t ) < tie ( b . gnss , b . sv , b . t ) ;
}
void readSP3s ( string_view fname )
{
SP3Reader sp3 ( fname ) ;
SP3Entry e ;
while ( sp3 . get ( e ) ) {
g_sp3s . push_back ( e ) ;
}
sort ( g_sp3s . begin ( ) , g_sp3s . end ( ) , sp3Order ) ;
}
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-22 09:48:02 -06:00
// 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-10-01 07:03:30 -06:00
struct SVFilter
{
struct SatID
{
int gnss , sv , sigid ;
bool operator < ( const SatID & rhs ) const
{
return std : : tie ( gnss , sv , sigid ) < std : : tie ( rhs . gnss , rhs . sv , rhs . sigid ) ;
}
} ;
void addFilter ( string_view str )
{
SatID satid { 0 , 0 , - 1 } ;
satid . gnss = atoi ( & str [ 0 ] ) ;
auto pos = str . find ( ' , ' ) ;
if ( pos ! = string : : npos )
satid . sv = atoi ( & str [ pos + 1 ] ) ;
pos = str . find ( ' , ' , pos + 1 ) ;
if ( pos ! = string : : npos )
satid . sigid = atoi ( & str [ pos + 1 ] ) ;
d_filter . insert ( satid ) ;
}
bool check ( int gnss , int sv , int sigid = - 1 )
{
if ( d_filter . empty ( ) )
return true ;
if ( d_filter . count ( { gnss , 0 , - 1 } ) ) // gnss match
return true ;
if ( d_filter . count ( { gnss , sv , - 1 } ) ) // gnss, sv match
return true ;
if ( d_filter . count ( { gnss , sv , sigid } ) ) // gnss, sv match, sigid
return true ;
return false ;
}
set < SatID > d_filter ;
} ;
2020-01-09 06:33:02 -07:00
struct FixStat
{
double iTow { - 1 } ;
struct SatStat
{
double bestrange1 { - 1 } ; // corrected for clock
double bestrange5 { - 1 } ; // corrected for clock
double doppler1 { - 1 } ; // corrected for clock
double doppler5 { - 1 } ; // corrected for clock
double radvel { - 1 } ;
double ephrange { - 1 } ;
GalileoMessage ephemeris ;
} ;
map < pair < int , int > , SatStat > sats ;
} ;
double g_rcvoffset ;
void emitFixState ( int src , double iTow , FixStat & fs , int n )
{
cout < < " \n Fix state for source " < < src < < " , have " < < fs . sats . size ( ) < < " satellites, n= " < < n < < endl ;
// for(double dt = -0.2; dt < 0.2; dt+=0.0001)
double dt = 0 ;
double offset = 0 ;
{
int count = 0 ;
for ( const auto & s : fs . sats ) {
if ( s . first . second = = 14 | | s . first . second = = 18 )
continue ;
Point sat ;
double E = getCoordinates ( iTow , s . second . ephemeris , & sat ) ;
if ( getElevationDeg ( sat , g_ourpos ) < 20 )
continue ;
/*
Point sat ;
auto [ toffset , trend ] = s . second . ephemeris . getAtomicOffset ( iTow + dt ) ;
( void ) trend ;
getCoordinates ( iTow + toffset / 1000000000.0 + dt , s . second . ephemeris , & sat ) ;
double range = Vector ( g_ourpos , sat ) . length ( ) ;
getCoordinates ( iTow + range / 299792458.0 + toffset / 1000000000.0 + dt , s . second . ephemeris , & sat ) ;
range = Vector ( g_ourpos , sat ) . length ( ) ;
*/
double range = s . second . ephrange ;
if ( s . second . bestrange1 ! = - 1 ) {
offset + = s . second . bestrange1 - range ;
count + + ;
}
if ( s . second . bestrange5 ! = - 1 ) {
offset + = s . second . bestrange5 - range ;
count + + ;
}
}
if ( ! count ) {
fs . sats . clear ( ) ;
return ;
}
cout < < " dt " < < dt < < " err " < < offset < < " " < < count < < " avg " < < offset / count < < " " < < offset / count / 3e5 < < " ms " < < endl ;
offset / = count ;
}
for ( const auto & s : fs . sats ) {
Point sat ;
double E = getCoordinates ( iTow , s . second . ephemeris , & sat ) ;
cout < < " " < < s . first . first < < " , " < < s . first . second < < " : " < < s . second . bestrange1 - offset < < " " < < s . second . bestrange5 - offset < < " " < < s . second . ephrange < < " , delta1 " < < ( s . second . bestrange1 - offset - s . second . ephrange ) < < " , delta5 " < < ( s . second . bestrange5 - offset - s . second . ephrange ) < < " dd " < < s . second . bestrange1 - s . second . bestrange5 < < " t0e " < < s . second . ephemeris . getT0e ( ) < < " elev " < < getElevationDeg ( sat , g_ourpos ) < < " E " < < E < < " clock " < < s . second . ephemeris . getAtomicOffset ( iTow ) . first / 1000000 < < " ms doppler1 " < < s . second . doppler1 < < " doppler5 " < < s . second . doppler5 < < " radvel " < < s . second . radvel < < " frac " < < ( s . second . bestrange1 - offset - s . second . ephrange ) / s . second . radvel ;
cout < < " fixed " < < ( ( s . second . bestrange1 - offset - s . second . ephrange ) + ( s . second . ephrange / 299792458.0 ) * s . second . radvel ) < < " BGD-ns " < < ldexp ( s . second . ephemeris . BGDE1E5b , - 32 ) * 1000000000 < < endl ;
}
fs . sats . clear ( ) ;
}
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-10-01 07:03:30 -06:00
GOOGLE_PROTOBUF_VERIFY_VERSION ;
2020-01-21 11:13:29 -07:00
CLI : : App app ( program ) ;
2019-10-01 07:03:30 -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-10-05 14:24:15 -06:00
/*
2019-10-01 07:03:30 -06:00
readSP3s ( " all.sp3 " ) ;
if ( ! g_sp3s . empty ( ) ) {
// sort(g_sp3s.begin(), g_sp3s.end(), [](const auto& a, const auto&b) { return a.t < b.t; });
2019-10-05 14:24:15 -06:00
cout < < " Have " < < g_sp3s . size ( ) < < " sp3 entries " < < endl ; //, from "<<humanTime(g_sp3s.begin()->t) <<" to "<< humanTime(g_sp3s.rbegin()->t)<<endl;
2019-10-01 07:03:30 -06:00
}
2019-10-05 14:24:15 -06:00
*/
2019-10-01 07:03:30 -06:00
vector < string > svpairs ;
2020-01-09 06:33:02 -07:00
vector < int > stations ;
2019-10-01 07:03:30 -06:00
bool doReceptionData { false } ;
2019-10-05 14:24:15 -06:00
bool doRFData { true } ;
2019-10-01 07:03:30 -06:00
bool doObserverPosition { false } ;
2020-01-21 11:13:29 -07:00
bool doVERSION { false } ;
2019-10-01 07:03:30 -06:00
app . add_option ( " --svs " , svpairs , " Listen to specified svs. '0' = gps, '2' = Galileo, '2,1' is E01 " ) ;
2020-01-09 06:33:02 -07:00
app . add_option ( " --stations " , stations , " Listen to specified stations. " ) ;
2019-11-03 12:13:31 -07:00
app . add_option ( " --positions,-p " , doObserverPosition , " Print out observer positions (or not) " ) ;
app . add_option ( " --rfdata,-r " , doRFData , " Print out RF data (or not) " ) ;
2020-01-21 11:13:29 -07:00
app . add_flag ( " --version " , doVERSION , " show program version and copyright " ) ;
2019-11-03 12:13:31 -07:00
2019-10-01 07:03:30 -06:00
try {
app . parse ( argc , argv ) ;
} catch ( const CLI : : Error & e ) {
return app . exit ( e ) ;
}
2020-01-21 11:13:29 -07:00
if ( doVERSION ) {
2020-01-22 15:45:29 -07:00
showVersion ( program , g_gitHash ) ;
2020-01-21 11:13:29 -07:00
exit ( 0 ) ;
}
2019-10-01 07:03:30 -06:00
SVFilter svfilter ;
for ( const auto & svp : svpairs ) {
svfilter . addFilter ( svp ) ;
}
2020-01-09 06:33:02 -07:00
set < int > statset ;
for ( const auto & i : stations )
statset . insert ( i ) ;
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-10-01 07:03:30 -06:00
ofstream sp3csv ;
sp3csv . open ( " sp3.csv " , std : : ofstream : : out | std : : ofstream : : app ) ;
2020-01-09 06:33:02 -07:00
sp3csv < < " timestamp gnssid sv ephAge sp3X sp3Y sp3Z ephX ephY ephZ sp3Clock ephClock distance along clockDelta E speed " < < endl ;
ofstream loccsv ;
loccsv . open ( " jeff.csv " , std : : ofstream : : out | std : : ofstream : : app ) ;
//loccsv<<"timestamp lat lon altitude accuracy\n";
2020-02-04 13:25:20 -07:00
// RINEXNavWriter rnw("test.rnx");
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-12-20 15:03:12 -07:00
// I am so sorry
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
2020-01-09 06:33:02 -07:00
if ( ! statset . empty ( ) & & ! statset . count ( nmm . sourceid ( ) ) )
continue ;
2019-09-02 08:13:49 -06:00
// if(nmm.type() == NavMonMessage::ReceptionDataType)
// continue;
2019-08-30 09:56:41 -06:00
2019-10-01 07:03:30 -06:00
auto etstamp = [ & nmm ] ( ) {
cout < < humanTime ( nmm . localutcseconds ( ) , nmm . localutcnanoseconds ( ) ) < < " src " < < nmm . sourceid ( ) < < " " ;
uint32_t imptow = ( ( uint32_t ) round ( nmm . localutcseconds ( ) + nmm . localutcnanoseconds ( ) / 1000000000.0 ) - 935280000 + 18 ) % ( 7 * 86400 ) ;
if ( ! ( imptow % 2 ) )
imptow - - ;
cout < < " imptow " < < imptow - 2 < < " " ;
} ;
2019-08-30 09:56:41 -06:00
2020-01-09 06:33:02 -07:00
static map < int , GalileoMessage > galEphemeris ;
2019-08-12 17:15:25 -06:00
if ( nmm . type ( ) = = NavMonMessage : : ReceptionDataType ) {
2019-10-01 07:03:30 -06:00
if ( doReceptionData ) {
etstamp ( ) ;
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 ) {
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 ( ) ;
2019-10-01 07:03:30 -06:00
if ( ! svfilter . check ( 2 , sv , nmm . gi ( ) . sigid ( ) ) )
continue ;
etstamp ( ) ;
int64_t imptow = ( ( ( uint32_t ) round ( nmm . localutcseconds ( ) + nmm . localutcnanoseconds ( ) / 1000000000.0 ) - 935280000 + 18 ) % ( 7 * 86400 ) ) - 2 ;
if ( nmm . gi ( ) . sigid ( ) ! = 5 ) {
if ( ! ( imptow % 2 ) )
imptow - - ;
}
else if ( ( imptow % 2 ) )
imptow - - ;
if ( imptow ! = nmm . gi ( ) . gnsstow ( ) )
cout < < " !! " < < ( imptow - nmm . gi ( ) . gnsstow ( ) ) < < " !! " ;
2019-09-02 08:13:49 -06:00
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-10-01 07:03:30 -06:00
cout < < " gal inav wtype " < < wtype < < " for " < < nmm . gi ( ) . gnssid ( ) < < " , " < < nmm . gi ( ) . gnsssv ( ) < < " , " < < nmm . gi ( ) . sigid ( ) < < " pbwn " < < nmm . gi ( ) . gnsswn ( ) < < " pbtow " < < nmm . gi ( ) . gnsstow ( ) ;
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 ) {
2020-01-09 06:33:02 -07:00
2019-08-30 09:56:41 -06:00
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 ;
2019-10-01 07:03:30 -06:00
2020-01-09 06:33:02 -07:00
galEphemeris [ sv ] = gm ;
2020-02-04 13:25:20 -07:00
SatID sid ;
sid . gnss = 2 ;
sid . sv = sv ;
sid . sigid = 1 ;
int start = utcFromGST ( gm . wn , ( int ) gm . tow ) ;
2019-10-01 07:03:30 -06:00
SP3Entry e { 2 , sv , start } ;
auto bestSP3 = lower_bound ( g_sp3s . begin ( ) , g_sp3s . end ( ) , e , sp3Order ) ;
if ( bestSP3 ! = g_sp3s . end ( ) & & bestSP3 - > gnss = = e . gnss & & bestSP3 - > sv = = sv ) {
static set < pair < int , int > > haveSeen ;
if ( ! haveSeen . count ( { sv , bestSP3 - > t } ) ) {
haveSeen . insert ( { sv , bestSP3 - > t } ) ;
Point newPoint ;
2020-01-09 06:33:02 -07:00
double E = getCoordinates ( gm . tow + ( bestSP3 - > t - start ) , gm , & newPoint , false ) ;
2019-10-01 07:03:30 -06:00
Point sp3Point ( bestSP3 - > x , bestSP3 - > y , bestSP3 - > z ) ;
Vector dist ( newPoint , sp3Point ) ;
2020-01-09 06:33:02 -07:00
Vector nspeed ;
getSpeed ( gm . tow + ( bestSP3 - > t - start ) , gm , & nspeed ) ;
Vector speed = nspeed ;
nspeed . norm ( ) ;
double along = nspeed . inner ( dist ) ;
2019-10-01 07:03:30 -06:00
cout < < " \n sp3 " < < ( bestSP3 - > t - start ) < < " E " < < sv < < " " < < humanTime ( bestSP3 - > t ) < < " ( " < < newPoint . x / 1000.0 < < " , " < < newPoint . y / 1000.0 < < " , " < < newPoint . z / 1000.0 < < " ) ( " < <
( bestSP3 - > x / 1000.0 ) < < " , " < < ( bestSP3 - > y / 1000.0 ) < < " , " < < ( bestSP3 - > z / 1000.0 ) < < " ) " < < bestSP3 - > clockBias < < " " < < gm . getAtomicOffset ( gm . tow + ( bestSP3 - > t - start ) ) . first < < " " < < dist . length ( ) < < " " ;
cout < < ( bestSP3 - > clockBias - gm . getAtomicOffset ( gm . tow + ( bestSP3 - > t - start ) ) . first ) ;
cout < < " " < < gm . af0 < < " " < < gm . af1 ;
cout < < endl ;
sp3csv < < std : : fixed < < bestSP3 - > t < < " 2 " < < sv < < " " < < ephAge ( gm . tow + ( bestSP3 - > t - start ) , gm . getT0e ( ) ) < < " " < < bestSP3 - > x < < " " < < bestSP3 - > y < < " " < < bestSP3 - > z < < " " < < newPoint . x < < " " < < newPoint . y < < " " < < newPoint . z < < " " < < bestSP3 - > clockBias < < " " ;
2020-01-09 06:33:02 -07:00
sp3csv < < gm . getAtomicOffset ( gm . tow + ( bestSP3 - > t - start ) ) . first < < " " < < dist . length ( ) < < " " < < along < < " " ;
sp3csv < < ( bestSP3 - > clockBias - gm . getAtomicOffset ( gm . tow + ( bestSP3 - > t - start ) ) . first ) < < " " < < E < < " " < < speed . length ( ) < < endl ;
2019-10-01 07:03:30 -06:00
}
}
2019-09-22 09:48:02 -06:00
if ( ! oldEph [ sv ] . sqrtA )
oldEph [ sv ] = gm ;
else if ( oldEph [ sv ] . iodnav ! = gm . iodnav ) {
2020-02-04 13:25:20 -07:00
// rnw.emitEphemeris(sid, gm);
2019-09-22 09:48:02 -06:00
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 " ;
2019-11-03 12:13:31 -07:00
// doOrbitDump(2, sv, gm.wn, oldEph[sv], gm, gm.tow - 3*3600, gm.tow + 3*3600);
2019-10-01 07:03:30 -06:00
2019-09-22 09:48:02 -06:00
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-10-01 07:03:30 -06:00
if ( wtype = = 3 )
cout < < " sisa " < < ( int ) gm . sisa ;
2019-09-18 06:17:41 -06:00
}
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-10-01 07:03:30 -06:00
if ( wtype = = 0 | | wtype = = 5 | | wtype = = 6 ) {
if ( wtype ! = 0 | | gm . sparetime = = 2 ) {
cout < < " tow " < < gm . tow ;
tow = gm . tow ;
}
}
if ( wtype = = 5 ) {
cout < < " e1bhs " < < ( int ) gm . e1bhs < < " e5bhs " < < ( int ) gm . e5bhs < < " e1bdvs " < < ( int ) gm . e1bdvs < < " e5bdvs " < < ( int ) gm . e5bdvs < < " wn " < < gm . wn < < " ai0 " < < gm . ai0 < < " ai1 " < < gm . ai1 < < " ai2 " < < gm . ai2 < < " BGDE1E5a " < < gm . BGDE1E5a < < " BGDE1E5b " < < gm . BGDE1E5b ;
// wn = gm.wn;
}
if ( wtype = = 6 ) {
cout < < " a0 " < < gm . a0 < < " a1 " < < gm . a1 < < " t0t " < < gm . t0t < < " dtLS " < < ( int ) gm . dtLS ;
}
2019-08-29 04:45:37 -06:00
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-09 12:49:31 -06:00
2019-08-12 17:15:25 -06:00
int sv = nmm . gpsi ( ) . gnsssv ( ) ;
2019-10-01 07:03:30 -06:00
if ( ! svfilter . check ( 0 , sv ) )
continue ;
etstamp ( ) ;
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 ;
2020-01-12 13:41:33 -07:00
cout < < " gpshealth = " < < ( int ) gs . gpshealth < < " , wn " < < gs . wn < < " t0c " < < gs . t0c < < " af0 " < < gs . af0 < < " af1 " < < gs . af1 < < " af2 " < < gs . af2 ;
2019-08-30 09:56:41 -06:00
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 ) ;
2020-01-12 13:41:33 -07:00
cout < < " Timejump: " < < oldOffset . first - newOffset . first < < " after " < < ephAge ( getT0c ( gs ) , getT0c ( iter - > second ) ) < < " seconds, old t0c " < < iter - > second . t0c ;
2019-08-30 09:56:41 -06:00
}
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 ( ) ;
}
2019-12-20 15:03:12 -07:00
if ( page = = 18 )
cout < < " dtLS " < < ( int ) gs . dtLS < < " dtLSF " < < ( int ) gs . dtLSF ;
2019-09-09 12:49:31 -06:00
}
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-10-01 07:03:30 -06:00
int sv = nmm . bid1 ( ) . gnsssv ( ) ;
if ( ! svfilter . check ( 3 , sv ) )
2019-09-09 12:49:31 -06:00
continue ;
2019-10-01 07:03:30 -06:00
etstamp ( ) ;
2019-09-09 12:49:31 -06:00
2019-10-01 07:03:30 -06:00
2019-08-29 08:46:03 -06:00
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-12-20 15:03:12 -07:00
else if ( bm . fraid = = 5 & & pageno = = 10 ) {
cout < < " dTLS " < < ( int ) bm . deltaTLS ;
}
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 ( ) ;
2019-10-01 07:03:30 -06:00
if ( ! svfilter . check ( 3 , sv ) )
continue ;
etstamp ( ) ;
2019-08-29 08:46:03 -06:00
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-10-01 07:03:30 -06:00
if ( ! svfilter . check ( 6 , nmm . gloi ( ) . gnsssv ( ) ) )
2019-09-09 12:49:31 -06:00
continue ;
2019-10-01 07:03:30 -06:00
etstamp ( ) ;
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 ) {
2020-01-09 06:33:02 -07:00
g_ourpos = Point ( nmm . op ( ) . x ( ) , nmm . op ( ) . y ( ) , nmm . op ( ) . z ( ) ) ;
2019-10-01 07:03:30 -06:00
if ( ! doObserverPosition )
continue ;
etstamp ( ) ;
2019-12-02 10:36:47 -07:00
auto latlonh = ecefToWGS84 ( nmm . op ( ) . x ( ) , nmm . op ( ) . y ( ) , nmm . op ( ) . z ( ) ) ;
cout < < std : : fixed < < " ECEF " < < nmm . op ( ) . x ( ) < < " , " < < nmm . op ( ) . y ( ) < < " , " < < nmm . op ( ) . z ( ) ;
cout < < " , WGS84 lon " < < 180 * std : : get < 1 > ( latlonh ) / M_PI
< < " lat " < < 180 * std : : get < 0 > ( latlonh ) / M_PI
< < " elev " < < std : : get < 2 > ( latlonh ) < < " acc " < < nmm . op ( ) . acc ( ) < < " m " < < endl ;
2020-01-09 06:33:02 -07:00
loccsv < < std : : fixed < < nmm . localutcseconds ( ) + nmm . localutcnanoseconds ( ) / 1000000000.0 < < " " < < 180 * std : : get < 1 > ( latlonh ) / M_PI < < " " < <
180 * std : : get < 0 > ( latlonh ) / M_PI < < " " < < std : : get < 2 > ( latlonh ) < < " " < < nmm . op ( ) . acc ( ) < < " \n " ;
2019-08-12 17:15:25 -06:00
}
else if ( nmm . type ( ) = = NavMonMessage : : RFDataType ) {
2019-10-01 07:03:30 -06:00
if ( ! doRFData )
continue ;
2020-01-09 06:33:02 -07:00
if ( ! svfilter . check ( nmm . rfd ( ) . gnssid ( ) , nmm . rfd ( ) . gnsssv ( ) , nmm . gi ( ) . sigid ( ) ) )
continue ;
2019-10-01 07:03:30 -06:00
etstamp ( ) ;
2020-01-09 06:33:02 -07:00
cout < < std : : fixed < < " RFdata for " < < nmm . rfd ( ) . gnssid ( ) < < " , " < < nmm . rfd ( ) . gnsssv ( ) < < " , " < < ( nmm . rfd ( ) . has_sigid ( ) ? nmm . rfd ( ) . sigid ( ) : 0 ) < < " : " ;
2019-10-05 14:24:15 -06:00
cout < < " doppler-hz " < < nmm . rfd ( ) . doppler ( ) ;
cout < < " pseudo-range " < < nmm . rfd ( ) . pseudorange ( ) ;
2020-01-09 06:33:02 -07:00
cout < < " carrier-phase " < < nmm . rfd ( ) . carrierphase ( ) ;
cout < < " rcv-tow " < < nmm . rfd ( ) . rcvtow ( ) ;
2019-10-05 14:24:15 -06:00
cout < < " pr-std " < < nmm . rfd ( ) . prstd ( ) ;
cout < < " dop-std " < < nmm . rfd ( ) . dostd ( ) ;
cout < < " cp-std " < < nmm . rfd ( ) . cpstd ( ) ;
cout < < " locktime-ms " < < nmm . rfd ( ) . locktimems ( ) ;
2019-11-26 11:12:37 -07:00
if ( nmm . rfd ( ) . has_cno ( ) ) {
cout < < " cno-db " < < nmm . rfd ( ) . cno ( ) ;
}
2020-01-09 06:33:02 -07:00
if ( nmm . rfd ( ) . has_prvalid ( ) ) {
cout < < " prvalid " < < nmm . rfd ( ) . prvalid ( ) ;
}
if ( nmm . rfd ( ) . has_cpvalid ( ) ) {
cout < < " cpvalid " < < nmm . rfd ( ) . cpvalid ( ) ;
}
if ( nmm . rfd ( ) . has_clkreset ( ) ) {
cout < < " clkreset " < < nmm . rfd ( ) . clkreset ( ) ;
}
static map < int , FixStat > fixes ;
if ( nmm . rfd ( ) . gnssid ( ) = = 2 & & galEphemeris . count ( nmm . rfd ( ) . gnsssv ( ) ) ) { // galileo
const auto & eph = galEphemeris [ nmm . rfd ( ) . gnsssv ( ) ] ;
Point sat ;
auto [ offset , trend ] = eph . getAtomicOffset ( round ( nmm . rfd ( ) . rcvtow ( ) ) ) ;
( void ) trend ;
static int n ;
double E = getCoordinates ( nmm . rfd ( ) . rcvtow ( ) , eph , & sat ) ;
double range = Vector ( g_ourpos , sat ) . length ( ) ;
double origrange = range ;
E = getCoordinates ( nmm . rfd ( ) . rcvtow ( ) - range / 299792458.0 , eph , & sat ) ;
range = Vector ( g_ourpos , sat ) . length ( ) ;
cout < < " d " < < origrange - range ;
origrange = range ;
/*
E = getCoordinates ( nmm . rfd ( ) . rcvtow ( ) + range / 299792458.0 + offset / 1000000000.0 - 0.018 , eph , & sat ) ;
range = Vector ( g_ourpos , sat ) . length ( ) ;
cout < < " d " < < 10000.0 * ( origrange - range ) ;
origrange = range ;
*/
constexpr double omegaE = 2 * M_PI / 86164.091 ;
/*
double theta = - ( 2 * M_PI * range / 299792458.0 ) / 86164.091 ; // sidereal day
Point rot ;
rot . x = sat . x * cos ( theta ) - sat . y * sin ( theta ) ;
rot . y = sat . x * sin ( theta ) + sat . y * cos ( theta ) ;
rot . z = sat . z ;
double oldrange = range ;
range = Vector ( g_ourpos , rot ) . length ( ) ; // second try
cout < < " rot-shift " < < oldrange - range < < " abs-move " < < Vector ( rot , sat ) . length ( ) ;
*/
double rotcor = omegaE * ( sat . x * g_ourpos . y - sat . y * g_ourpos . x ) / 299792458.0 ;
cout < < " rot-shift " < < rotcor ;
range + = rotcor ;
double bestrange = nmm . rfd ( ) . pseudorange ( ) ;
double gap = range - bestrange ;
cout < < " pseudo-gap " < < gap / 1000.0 ;
constexpr double speedOfLightPerNS = 299792458.0 / 1000000000.0 ;
bestrange + = speedOfLightPerNS * offset ;
// Δtr=F e A1/2 sin(E)
constexpr double F = 1000000000.0 * - 4.442807309e-10 ; // "in ns"
double dtr = F * eph . getE ( ) * eph . getSqrtA ( ) * sin ( E ) ;
bestrange - = speedOfLightPerNS * dtr ;
cout < < " relcor " < < speedOfLightPerNS * dtr ;
// multi-freq adjustment is done in emitFixState
cout < < " clockcor " < < offset / 1000000.0 < < " ms gap " < < gap / 1000.0 ;
if ( fixes [ nmm . sourceid ( ) ] . iTow ! = nmm . rfd ( ) . rcvtow ( ) ) {
emitFixState ( nmm . sourceid ( ) , nmm . rfd ( ) . rcvtow ( ) , fixes [ nmm . sourceid ( ) ] , n ) ;
fixes [ nmm . sourceid ( ) ] . iTow = nmm . rfd ( ) . rcvtow ( ) ;
n + + ;
}
auto & satstat = fixes [ nmm . sourceid ( ) ] . sats [ { ( int ) nmm . rfd ( ) . gnssid ( ) , ( int ) nmm . rfd ( ) . gnsssv ( ) } ] ;
satstat . ephrange = range ;
auto dop = doDoppler ( nmm . rfd ( ) . rcvtow ( ) , g_ourpos , eph , 1575420000 ) ;
satstat . radvel = dop . radvel ;
if ( nmm . rfd ( ) . sigid ( ) = = 1 ) {
satstat . bestrange1 = bestrange ;
satstat . doppler1 = nmm . rfd ( ) . doppler ( ) ;
}
else if ( nmm . rfd ( ) . sigid ( ) = = 5 ) {
satstat . bestrange5 = bestrange ;
satstat . doppler5 = nmm . rfd ( ) . doppler ( ) ;
}
satstat . ephemeris = eph ;
}
2019-10-05 14:24:15 -06:00
cout < < endl ;
2020-01-09 06:33:02 -07:00
}
else if ( nmm . type ( ) = = NavMonMessage : : ObserverDetailsType ) {
etstamp ( ) ;
2020-01-09 07:25:38 -07:00
cout < < " vendor " < < nmm . od ( ) . vendor ( ) < < " hwversion " < < nmm . od ( ) . hwversion ( ) < < " modules " < < nmm . od ( ) . modules ( ) < < " swversion " < < nmm . od ( ) . swversion ( ) ;
cout < < " serial " < < nmm . od ( ) . serialno ( ) ;
if ( nmm . od ( ) . has_owner ( ) )
cout < < " owner " < < nmm . od ( ) . owner ( ) ;
if ( nmm . od ( ) . has_clockoffsetdriftns ( ) )
cout < < " drift " < < nmm . od ( ) . clockoffsetdriftns ( ) ;
if ( nmm . od ( ) . has_clockaccuracyns ( ) )
cout < < " clock-accuracy " < < nmm . od ( ) . clockaccuracyns ( ) ;
cout < < endl ;
2020-01-09 06:33:02 -07:00
}
2020-01-29 10:06:23 -07:00
else if ( nmm . type ( ) = = NavMonMessage : : UbloxJammingStatsType ) {
etstamp ( ) ;
cout < < " noisePerMS " < < nmm . ujs ( ) . noiseperms ( ) < < " agcCnt " < <
nmm . ujs ( ) . agccnt ( ) < < " flags " < < nmm . ujs ( ) . flags ( ) < < " jamind " < <
nmm . ujs ( ) . jamind ( ) < < endl ;
}
2020-01-09 06:33:02 -07:00
else if ( nmm . type ( ) = = NavMonMessage : : DebuggingType ) {
2020-01-09 07:25:38 -07:00
2020-01-09 06:33:02 -07:00
auto res = parseTrkMeas ( basic_string < uint8_t > ( ( const uint8_t * ) nmm . dm ( ) . payload ( ) . c_str ( ) , nmm . dm ( ) . payload ( ) . size ( ) ) ) ;
2020-01-09 07:25:38 -07:00
if ( res . empty ( ) )
continue ;
etstamp ( ) ;
2020-01-09 06:33:02 -07:00
uint64_t maxt = 0 ;
for ( const auto & sv : res ) {
if ( sv . gnss ! = 2 ) continue ;
if ( sv . tr > maxt )
maxt = sv . tr ;
}
double ttag = round ( ( ldexp ( 1.0 * maxt , - 32 ) / 1000.0 + 0.08 ) / 0.1 ) * 0.1 ;
double rtow = round ( ldexp ( 1.0 * maxt , - 32 ) / 1000.0 ) ; // this was the rounded tow of transmission
sort ( res . begin ( ) , res . end ( ) , [ & ] ( const auto & a , const auto & b ) {
double elevA = 0 , elevB = 0 ;
if ( galEphemeris . count ( a . sv ) ) {
const auto & eph = galEphemeris [ a . sv ] ;
Point sat ;
getCoordinates ( rtow , eph , & sat ) ;
elevA = getElevationDeg ( sat , g_ourpos ) ;
}
if ( galEphemeris . count ( b . sv ) ) {
const auto & eph = galEphemeris [ b . sv ] ;
Point sat ;
getCoordinates ( rtow , eph , & sat ) ;
elevB = getElevationDeg ( sat , g_ourpos ) ;
}
return elevB < elevA ;
} ) ;
double toffsetms = 0 ;
bool first = true ;
for ( const auto sv : res ) {
if ( sv . gnss ! = 2 ) continue ;
if ( ! galEphemeris . count ( sv . sv ) )
continue ;
const auto & eph = galEphemeris [ sv . sv ] ;
double clockoffms = eph . getAtomicOffset ( rtow ) . first / 1000000.0 ;
Point sat ;
double E = getCoordinates ( rtow - clockoffms / 1000.0 , eph , & sat , sv . sv ! = 14 ) ;
double range = Vector ( g_ourpos , sat ) . length ( ) ;
getCoordinates ( rtow - clockoffms / 1000.0 - range / 299792458.0 , eph , & sat ) ;
range = Vector ( g_ourpos , sat ) . length ( ) ;
double trmsec = ldexp ( maxt - sv . tr , - 32 ) + clockoffms ;
constexpr double omegaE = 2 * M_PI / 86164.091 ;
double rotcor = omegaE * ( sat . x * g_ourpos . y - sat . y * g_ourpos . x ) / 299792458.0 ;
range + = rotcor ;
double bgdcor = 299792458.0 * ldexp ( eph . BGDE1E5b , - 32 ) ;
range - = bgdcor ;
constexpr double speedOfLightPerNS = 299792458.0 / 1000000000.0 ;
// Δtr=F e A1/2 sin(E)
constexpr double F = 1000000000.0 * - 4.442807309e-10 ; // "in ns"
double dtr = F * eph . getE ( ) * eph . getSqrtA ( ) * sin ( E ) ;
double relcor = speedOfLightPerNS * dtr ;
range - = relcor ;
double predTrmsec = range / 299792.4580 ;
if ( first ) {
toffsetms = trmsec - predTrmsec ;
first = false ;
cout < < " Set toffsetms to " < < toffsetms < < " for tow " < < rtow < < " ttag " < < ttag < < " raw " < < ( ldexp ( 1.0 * maxt , - 32 ) / 1000.0 ) < < endl ;
}
trmsec - = toffsetms ;
cout < < std : : fixed < < " gnssid " < < sv . gnss < < " sv " < < sv . sv < < " : doppler " < < sv . dopplerHz < < " range-ms " < < range / 299792.4580 ;
cout < < " actual-ms " < < trmsec < < " delta " < < ( ( range / 299792.4580 - trmsec ) ) < < " delta-m " < < 299792.4580 * ( ( range / 299792.4580 - trmsec ) ) ;
cout < < " rotcor " < < rotcor ;
cout < < " relcor " < < relcor ;
cout < < " elev " < < getElevationDeg ( sat , g_ourpos ) ;
cout < < " bgd-m " < < bgdcor ;
cout < < " clockoff-ms " < < clockoffms < < endl ;
}
2020-01-12 13:41:33 -07:00
cout < < endl ;
2019-08-12 17:15:25 -06:00
}
else {
2019-10-01 07:03:30 -06:00
etstamp ( ) ;
2019-08-12 17:15:25 -06:00
cout < < " Unknown type " < < ( int ) nmm . type ( ) < < endl ;
}
}
}
2019-09-02 08:13:49 -06:00
catch ( EofException & ee )
{ }