1361 lines
49 KiB
C++
1361 lines
49 KiB
C++
#include <stdio.h>
|
|
#include <string>
|
|
#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 "ext/powerblog/h2o-pp.hh"
|
|
#include "minicurl.hh"
|
|
#include <time.h>
|
|
#include "ubx.hh"
|
|
#include "bits.hh"
|
|
#include "minivec.hh"
|
|
#include "navmon.pb.h"
|
|
#include "ephemeris.hh"
|
|
#include "gps.hh"
|
|
#include "glonass.hh"
|
|
#include "beidou.hh"
|
|
#include "galileo.hh"
|
|
#include "tle.hh"
|
|
#include <optional>
|
|
#include "navmon.hh"
|
|
#include <Tle.h>
|
|
using namespace std;
|
|
|
|
Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000);
|
|
|
|
map<int, BeidouAlmanacEntry> g_beidoualma;
|
|
map<int, pair<GlonassMessage, GlonassMessage>> g_glonassalma;
|
|
|
|
TLERepo g_tles;
|
|
struct GNSSReceiver
|
|
{
|
|
Point position;
|
|
};
|
|
|
|
|
|
int g_dtLS{18}, g_dtLSBeidou{4};
|
|
uint64_t utcFromGST(int wn, int tow)
|
|
{
|
|
return (935280000 + wn * 7*86400 + tow - g_dtLS);
|
|
}
|
|
|
|
double utcFromGST(int wn, double tow)
|
|
{
|
|
return (935280000.0 + wn * 7*86400 + tow - g_dtLS);
|
|
}
|
|
|
|
double utcFromGPS(int wn, double tow)
|
|
{
|
|
return (315964800 + wn * 7*86400 + tow - g_dtLS);
|
|
}
|
|
|
|
string humanFt(uint8_t ft)
|
|
{
|
|
static const char* ret[]={"100 cm", "200 cm", "250 cm", "400 cm", "500 cm", "7 m", "10 m", "12 m", "14 m", "16 m", "32 m", "64 m", "128 m", "256 m", "512 m", "NONE"};
|
|
if(ft < 16)
|
|
return ret[ft];
|
|
return "???";
|
|
}
|
|
|
|
|
|
string humanSisa(uint8_t sisa)
|
|
{
|
|
unsigned int sval = sisa;
|
|
if(sisa < 50)
|
|
return std::to_string(sval)+" cm";
|
|
if(sisa < 75)
|
|
return std::to_string(50 + 2* (sval-50))+" cm";
|
|
if(sisa < 100)
|
|
return std::to_string(100 + 4*(sval-75))+" cm";
|
|
if(sisa < 125)
|
|
return std::to_string(200 + 16*(sval-100))+" cm";
|
|
if(sisa < 255)
|
|
return "SPARE";
|
|
return "NO SIS AVAILABLE";
|
|
}
|
|
|
|
string humanUra(uint8_t ura)
|
|
{
|
|
if(ura < 6)
|
|
return fmt::sprintf("%d cm", (int)(100*pow(2.0, 1.0+1.0*ura/2.0)));
|
|
else if(ura < 15)
|
|
return fmt::sprintf("%d m", (int)(pow(2, ura-2)));
|
|
return "NO URA AVAILABLE";
|
|
}
|
|
|
|
|
|
struct SVIOD
|
|
{
|
|
std::bitset<32> words;
|
|
int gnssid;
|
|
uint32_t t0e;
|
|
uint32_t e, sqrtA;
|
|
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
|
|
|
|
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
|
|
// 60 seconds
|
|
uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS
|
|
|
|
// 2^-34 2^-46
|
|
int32_t af0{0} , af1{0};
|
|
// 2^-59
|
|
int8_t af2{0};
|
|
|
|
uint8_t sisa;
|
|
|
|
uint32_t wn{0}, tow{0};
|
|
bool complete() const
|
|
{
|
|
if(gnssid==2)
|
|
return words[1] && words[2] && words[3] && words[4];
|
|
else
|
|
return words[2] && words[3];
|
|
}
|
|
void addGalileoWord(std::basic_string_view<uint8_t> page);
|
|
|
|
double getMu() const
|
|
{
|
|
if(gnssid == 2) return 3.986004418 * pow(10.0, 14.0);
|
|
if(gnssid == 0) return 3.986005 * pow(10.0, 14.0);
|
|
throw std::runtime_error("getMu() called for unsupported gnssid "+to_string(gnssid));
|
|
} // m^3/s^2
|
|
// same for galileo & gps
|
|
double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
|
|
|
|
uint32_t getT0e() const { return t0e; }
|
|
double getSqrtA() const { return ldexp(sqrtA, -19); }
|
|
double getE() const { return ldexp(e, -33); }
|
|
double getCuc() const { return ldexp(cuc, -29); } // radians
|
|
double getCus() const { return ldexp(cus, -29); } // radians
|
|
double getCrc() const { return ldexp(crc, -5); } // meters
|
|
double getCrs() const { return ldexp(crs, -5); } // meters
|
|
double getM0() const { return ldexp(m0 * M_PI, -31); } // radians
|
|
double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s
|
|
double getI0() const { return ldexp(i0 * M_PI, -31); } // radians
|
|
double getCic() const { return ldexp(cic, -29); } // radians
|
|
double getCis() const { return ldexp(cis, -29); } // radians
|
|
double getOmegadot() const { return ldexp(omegadot * M_PI, -43); } // radians/s
|
|
double getOmega0() const { return ldexp(omega0 * M_PI, -31); } // radians
|
|
double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s
|
|
double getOmega() const { return ldexp(omega * M_PI, -31); } // radians
|
|
|
|
|
|
};
|
|
|
|
void SVIOD::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|
{
|
|
uint8_t wtype = getbitu(&page[0], 0, 6);
|
|
words[wtype]=true;
|
|
gnssid = 2;
|
|
if(wtype == 1) {
|
|
t0e = getbitu(&page[0], 16, 14) * 60; // WE SCALE THIS FOR THE USER!
|
|
m0 = getbits(&page[0], 30, 32);
|
|
e = getbitu(&page[0], 62, 32);
|
|
sqrtA = getbitu(&page[0], 94, 32);
|
|
}
|
|
else if(wtype == 2) {
|
|
omega0 = getbits(&page[0], 16, 32);
|
|
i0 = getbits(&page[0], 48, 32);
|
|
omega = getbits(&page[0], 80, 32);
|
|
idot = getbits(&page[0], 112, 14);
|
|
}
|
|
else if(wtype == 3) {
|
|
omegadot = getbits(&page[0], 16, 24);
|
|
deltan = getbits(&page[0], 40, 16);
|
|
cuc = getbits(&page[0], 56, 16);
|
|
cus = getbits(&page[0], 72, 16);
|
|
crc = getbits(&page[0], 88, 16);
|
|
crs = getbits(&page[0], 104, 16);
|
|
sisa = getbitu(&page[0], 120, 8);
|
|
}
|
|
else if(wtype == 4) {
|
|
cic = getbits(&page[0], 22, 16);
|
|
cis = getbits(&page[0], 38, 16);
|
|
|
|
t0c = getbitu(&page[0], 54, 14);
|
|
af0 = getbits(&page[0], 68, 31);
|
|
af1 = getbits(&page[0], 99, 21);
|
|
af2 = getbits(&page[0], 120, 6);
|
|
/*
|
|
cout<<(int) t0c << " " <<(int) af0 <<" " <<(int) af1 <<" " <<(int) af2<<endl;
|
|
cout<<(int) t0c*60 << " " << (((double) af0) / (1ULL<<34))*1000000 <<" usec " << (((double) af1)/(1ULL << 46))*1000000000000 <<" ps/s"<<endl;
|
|
*/
|
|
}
|
|
|
|
}
|
|
|
|
struct SVPerRecv
|
|
{
|
|
int el{-1}, azi{-1}, db{-1};
|
|
time_t t; // last seen
|
|
};
|
|
|
|
|
|
/* Most of thes fields are raw, EXCEPT:
|
|
t0t = seconds, raw fields are 3600 seconds (galileo), 4096 seconds (GPS)
|
|
*/
|
|
|
|
struct SVStat
|
|
{
|
|
uint8_t e5bhs{0}, e1bhs{0};
|
|
uint8_t gpshealth{0};
|
|
uint16_t ai0{0};
|
|
int16_t ai1{0}, ai2{0};
|
|
bool sf1{0}, sf2{0}, sf3{0}, sf4{0}, sf5{0};
|
|
int BGDE1E5a{0}, BGDE1E5b{0};
|
|
bool e5bdvs{false}, e1bdvs{false};
|
|
bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
|
|
uint16_t wn{0}; // we put the "unrolled" week number here!
|
|
uint32_t tow{0}; // "last seen"
|
|
//
|
|
// 2^-30 2^-50 1 8-bit week
|
|
int32_t a0{0}, a1{0}, t0t{0}, wn0t{0};
|
|
int32_t a0g{0}, a1g{0}, t0g{0}, wn0g{0};
|
|
int8_t dtLS{0}, dtLSF{0};
|
|
uint16_t wnLSF{0};
|
|
uint8_t dn; // leap second day number
|
|
// 1 2^-31 2^-43 2^-55 16 second
|
|
int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX
|
|
|
|
// beidou:
|
|
int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1};
|
|
BeidouMessage beidouMessage, oldBeidouMessage;
|
|
BeidouMessage lastBeidouMessage1, lastBeidouMessage2;
|
|
TLERepo::Match tleMatch;
|
|
|
|
// new galileo
|
|
GalileoMessage galmsg;
|
|
|
|
// Glonass
|
|
GlonassMessage glonassMessage;
|
|
pair<uint32_t, GlonassMessage> glonassAlmaEven;
|
|
|
|
map<uint64_t, SVPerRecv> perrecv;
|
|
pair<uint32_t, double> deltaHz;
|
|
double latestDisco{-1}, timeDisco{-1000};
|
|
|
|
map<int, SVIOD> iods;
|
|
void addGalileoWord(std::basic_string_view<uint8_t> page);
|
|
|
|
bool completeIOD() const;
|
|
uint16_t getIOD() const;
|
|
SVIOD liveIOD() const;
|
|
SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor
|
|
|
|
pair<int,SVIOD> prevIOD{-1, SVIOD()};
|
|
void clearPrev()
|
|
{
|
|
prevIOD.first = -1;
|
|
}
|
|
void checkCompleteAndClean(int iod);
|
|
};
|
|
|
|
bool SVStat::completeIOD() const
|
|
{
|
|
for(const auto& iod : iods)
|
|
if(iod.second.complete())
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
uint16_t SVStat::getIOD() const
|
|
{
|
|
for(const auto& iod : iods)
|
|
if(iod.second.complete())
|
|
return iod.first;
|
|
throw std::runtime_error("Asked for live IOD number, don't have one yet");
|
|
}
|
|
|
|
SVIOD SVStat::liveIOD() const
|
|
{
|
|
if(auto iter = iods.find(getIOD()); iter != iods.end())
|
|
return iter->second;
|
|
throw std::runtime_error("Asked for live IOD, don't have one yet");
|
|
}
|
|
|
|
void SVStat::checkCompleteAndClean(int iod)
|
|
{
|
|
if(iods[iod].complete()) {
|
|
for(const auto& i : iods) {
|
|
if(i.first != iod && i.second.complete())
|
|
prevIOD=i;
|
|
}
|
|
SVIOD latest = iods[iod];
|
|
|
|
decltype(iods) newiods; // XXX race condition here,
|
|
newiods[iod]=latest;
|
|
iods.swap(newiods); // try to keep it brief
|
|
}
|
|
}
|
|
|
|
void SVStat::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|
{
|
|
uint8_t wtype = getbitu(&page[0], 0, 6);
|
|
if(wtype == 0) {
|
|
if(getbitu(&page[0], 6,2) == 2) {
|
|
wn = getbitu(&page[0], 96, 12);
|
|
if(tow != getbitu(&page[0], 108, 20)) {
|
|
cerr<<"wtype "<<wtype<<", was about to mis-set TOW, " <<tow<< " " <<getbitu(&page[0], 108, 20) <<endl;
|
|
}
|
|
|
|
}
|
|
}
|
|
else if(wtype >=1 && wtype <= 4) { // ephemeris
|
|
uint16_t iod = getbitu(&page[0], 6, 10);
|
|
iods[iod].addGalileoWord(page);
|
|
checkCompleteAndClean(iod);
|
|
}
|
|
else if(wtype==5) { // disturbance, health, time
|
|
ai0 = getbitu(&page[0], 6, 11);
|
|
ai1 = getbits(&page[0], 17, 11); // ai1 & 2 are signed, 0 not
|
|
ai2 = getbits(&page[0], 28, 14);
|
|
|
|
sf1 = getbitu(&page[0], 42, 1);
|
|
sf2 = getbitu(&page[0], 43, 1);
|
|
sf3 = getbitu(&page[0], 44, 1);
|
|
sf4 = getbitu(&page[0], 45, 1);
|
|
sf5 = getbitu(&page[0], 46, 1);
|
|
BGDE1E5a = getbits(&page[0], 47, 10);
|
|
BGDE1E5b = getbits(&page[0], 57, 10);
|
|
|
|
e5bhs = getbitu(&page[0], 67, 2);
|
|
e1bhs = getbitu(&page[0], 69, 2);
|
|
e5bdvs = getbitu(&page[0], 71, 1);
|
|
e1bdvs = getbitu(&page[0], 72, 1);
|
|
wn = getbitu(&page[0], 73, 12);
|
|
if(tow != getbitu(&page[0], 85, 20))
|
|
{
|
|
cerr<<"wtype "<<wtype<<", was about to mis-set TOW"<<endl;
|
|
}
|
|
}
|
|
else if(wtype == 6) {
|
|
|
|
a0 = getbits(&page[0], 6, 32);
|
|
a1 = getbits(&page[0], 38, 24);
|
|
dtLS = getbits(&page[0], 62, 8);
|
|
t0t = getbitu(&page[0], 70, 8) * 3600; // WE SCALE THIS FOR THE USER
|
|
wn0t = getbitu(&page[0], 78, 8);
|
|
wnLSF = getbitu(&page[0], 86, 8);
|
|
dn = getbitu(&page[0], 94, 3);
|
|
dtLSF = getbits(&page[0], 97, 8);
|
|
|
|
// cout<<(int) dtLS << " " <<(int) wnLSF<<" " <<(int) dn <<" " <<(int) dtLSF<<endl;
|
|
if(tow != getbitu(&page[0], 105, 20)) {
|
|
cerr<<"wtype "<<wtype<<", was about to mis-set TOW"<<endl;
|
|
}
|
|
}
|
|
else if(wtype == 10) { // GSTT GPS
|
|
a0g = getbits(&page[0], 86, 16);
|
|
a1g = getbits(&page[0], 102, 12);
|
|
t0g = getbitu(&page[0], 114, 8);
|
|
wn0g = getbitu(&page[0], 122, 6);
|
|
}
|
|
}
|
|
|
|
template<typename T>
|
|
void getSpeed(int wn, double tow, const T& eph, Vector* v)
|
|
{
|
|
Point a, b;
|
|
getCoordinates(wn, tow-0.5, eph, &a);
|
|
getCoordinates(wn, tow+0.5, eph, &b);
|
|
*v = Vector(a, b);
|
|
}
|
|
|
|
std::map<pair<int,int>, SVStat> g_svstats;
|
|
|
|
int latestWN(int gnssid)
|
|
{
|
|
map<int, pair<int,int>> ages;
|
|
for(const auto& s: g_svstats)
|
|
if(s.first.first == gnssid)
|
|
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
|
|
if(ages.empty())
|
|
throw runtime_error("Asked for latest WN for "+to_string(gnssid)+": we don't know it yet");
|
|
return g_svstats[ages.rbegin()->second].wn;
|
|
}
|
|
|
|
int latestTow(int gnssid)
|
|
{
|
|
map<int, pair<int,int>> ages;
|
|
for(const auto& s: g_svstats)
|
|
if(s.first.first == gnssid)
|
|
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
|
|
if(ages.empty())
|
|
throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet");
|
|
return g_svstats[ages.rbegin()->second].tow;
|
|
}
|
|
|
|
|
|
|
|
uint64_t nanoTime(int gnssid, int wn, int tow)
|
|
{
|
|
int offset;
|
|
if(gnssid == 0) // GPS
|
|
offset = 315964800;
|
|
if(gnssid == 2) // Galileo, 22-08-1999
|
|
offset = 935280000;
|
|
if(gnssid == 3) {// Beidou, 01-01-2006 - I think leap seconds count differently in Beidou!! XXX
|
|
offset = 1136073600;
|
|
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLSBeidou);
|
|
}
|
|
if(gnssid == 6) { // GLONASS
|
|
offset = 820368000;
|
|
return 1000000000ULL*(offset + wn * 7*86400 + tow); // no leap seconds in glonass
|
|
}
|
|
|
|
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS);
|
|
}
|
|
|
|
|
|
|
|
struct InfluxPusher
|
|
{
|
|
explicit InfluxPusher(std::string_view dbname) : d_dbname(dbname)
|
|
{
|
|
if(dbname=="null")
|
|
cout<<"Not sending data to influxdb"<<endl;
|
|
}
|
|
template<typename T>
|
|
void addValue( const pair<pair<int,int>,SVStat>& ent, string_view name, const T& value)
|
|
{
|
|
d_buffer+= string(name)+",gnssid="+to_string(ent.first.first)+ +",sv=" +to_string(ent.first.second)+" value="+to_string(value)+
|
|
" "+to_string(nanoTime(ent.first.first, ent.second.wn, ent.second.tow))+"\n";
|
|
checkSend();
|
|
}
|
|
|
|
template<typename T>
|
|
void addValue(pair<int,int> id, string_view name, const T& value)
|
|
{
|
|
if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0)
|
|
return;
|
|
// cout << g_svstats[id].wn <<", "<<g_svstats[id].tow<<" -> " <<nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow)<<endl;
|
|
d_buffer+= string(name) +",gnssid="+to_string(id.first)+",sv=" +to_string(id.second) + " value="+to_string(value)+" "+
|
|
to_string(nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
|
|
|
checkSend();
|
|
}
|
|
|
|
void checkSend()
|
|
{
|
|
if(d_buffer.size() > 1000000 || (time(0) - d_lastsent) > 10) {
|
|
string buffer;
|
|
buffer.swap(d_buffer);
|
|
// thread t([buffer,this]() {
|
|
if(d_dbname != "null")
|
|
doSend(buffer);
|
|
// });
|
|
// t.detach();
|
|
d_lastsent=time(0);
|
|
}
|
|
}
|
|
|
|
void doSend(std::string buffer)
|
|
{
|
|
MiniCurl mc;
|
|
MiniCurl::MiniCurlHeaders mch;
|
|
if(!buffer.empty()) {
|
|
mc.postURL("http://127.0.0.1:8086/write?db="+d_dbname, buffer, mch);
|
|
}
|
|
}
|
|
|
|
~InfluxPusher()
|
|
{
|
|
if(d_dbname != "null")
|
|
doSend(d_buffer);
|
|
}
|
|
|
|
std::string d_buffer;
|
|
time_t d_lastsent{0};
|
|
string d_dbname;
|
|
};
|
|
|
|
/* The GST start epoch is defined as 13 seconds before midnight between 21st August and
|
|
22nd August 1999, i.e. GST was equal to 13 seconds at 22nd August 1999 00:00:00 UTC. */
|
|
|
|
std::string humanTime(int wn, int tow)
|
|
{
|
|
time_t t = utcFromGST(wn, tow);
|
|
struct tm tm;
|
|
gmtime_r(&t, &tm);
|
|
|
|
char buffer[80];
|
|
strftime(buffer, sizeof(buffer), "%a, %d %b %Y %T %z", &tm);
|
|
return buffer;
|
|
}
|
|
|
|
std::optional<double> getHzCorrection(time_t now)
|
|
{
|
|
int galcount{0}, gpscount{0}, allcount{0};
|
|
double galtot{0}, gpstot{0}, alltot{0};
|
|
|
|
for(const auto& s: g_svstats) {
|
|
if(now - s.second.deltaHz.first < 60) {
|
|
alltot+=s.second.deltaHz.second;
|
|
allcount++;
|
|
if(s.first.first == 0) {
|
|
gpstot+=s.second.deltaHz.second;
|
|
gpscount++;
|
|
}
|
|
else if(s.first.first == 2) {
|
|
galtot+=s.second.deltaHz.second;
|
|
galcount++;
|
|
}
|
|
}
|
|
}
|
|
std::optional<double> galHzCorr, gpsHzCorr, allHzCorr;
|
|
if(galcount > 3)
|
|
galHzCorr = galtot/galcount;
|
|
if(gpscount > 3)
|
|
gpsHzCorr = gpstot/gpscount;
|
|
if(allcount > 3)
|
|
allHzCorr = alltot/allcount;
|
|
|
|
if(galHzCorr)
|
|
return galHzCorr;
|
|
return allHzCorr;
|
|
}
|
|
|
|
char getGNSSChar(int id)
|
|
{
|
|
if(id==0)
|
|
return 'G';
|
|
if(id==2)
|
|
return 'E';
|
|
if(id==3)
|
|
return 'C';
|
|
if(id==6)
|
|
return 'R';
|
|
else
|
|
return '0'+id;
|
|
}
|
|
|
|
double getElevation(const Point& p)
|
|
{
|
|
Point our = g_ourpos;
|
|
Point core{0,0,0};
|
|
|
|
Vector core2us(core, our);
|
|
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
|
|
|
|
// https://ds9a.nl/articles/
|
|
|
|
double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
|
|
double deg = 180.0* (elev/M_PI);
|
|
return 90.0 - deg;
|
|
}
|
|
|
|
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
try
|
|
{
|
|
// g_tles.parseFile("active.txt");
|
|
|
|
g_tles.parseFile("galileo.txt");
|
|
g_tles.parseFile("glo-ops.txt");
|
|
g_tles.parseFile("gps-ops.txt");
|
|
g_tles.parseFile("beidou.txt");
|
|
|
|
signal(SIGPIPE, SIG_IGN);
|
|
InfluxPusher idb(argc > 3 ? argv[3] : "galileo");
|
|
MiniCurl::init();
|
|
|
|
H2OWebserver h2s("galmon");
|
|
|
|
|
|
h2s.addHandler("/global", [](auto handler, auto req) {
|
|
nlohmann::json ret = nlohmann::json::object();
|
|
ret["leap-seconds"] = g_dtLS;
|
|
try {
|
|
ret["last-seen"]=utcFromGST(latestWN(2), latestTow(2));
|
|
}
|
|
catch(...)
|
|
{}
|
|
|
|
map<int, int> utcstats, gpsgststats, gpsutcstats;
|
|
for(const auto& s: g_svstats) {
|
|
if(!s.second.wn) // this will suck in 20 years
|
|
continue;
|
|
|
|
//Galileo-UTC offset: 3.22 ns, Galileo-GPS offset: 7.67 ns, 18 leap seconds
|
|
|
|
|
|
if(s.first.first == 0) { // GPS
|
|
int sv = s.first.second;
|
|
int dw = (uint8_t)g_svstats[{0,sv}].wn - g_svstats[{0,sv}].wn0t;
|
|
int age = dw * 7 * 86400 + g_svstats[{0,sv}].tow - g_svstats[{0,sv}].t0t; // t0t is PRESCALED
|
|
|
|
gpsutcstats[age]=s.first.second;
|
|
continue;
|
|
}
|
|
|
|
int dw = (uint8_t)s.second.wn - s.second.wn0t;
|
|
int age = dw * 7 * 86400 + s.second.tow - s.second.t0t; // t0t is pre-scaled
|
|
utcstats[age]=s.first.second;
|
|
|
|
uint8_t wn0g = s.second.wn0t;
|
|
int dwg = (((uint8_t)s.second.wn)&(1+2+4+8+16+32)) - wn0g;
|
|
age = dwg*7*86400 + s.second.tow - s.second.t0g * 3600;
|
|
gpsgststats[age]=s.first.second;
|
|
|
|
}
|
|
if(utcstats.empty()) {
|
|
ret["utc-offset-ns"]=nullptr;
|
|
}
|
|
else {
|
|
int sv = utcstats.begin()->second; // freshest SV
|
|
long shift = g_svstats[{2,sv}].a0 * (1LL<<20) + g_svstats[{2,sv}].a1 * utcstats.begin()->first; // in 2^-50 seconds units
|
|
ret["utc-offset-ns"] = 1.073741824*ldexp(1.0*shift, -20);
|
|
ret["leap-second-planned"] = (g_svstats[{2,sv}].dtLSF != g_svstats[{2,sv}].dtLS);
|
|
}
|
|
|
|
if(gpsgststats.empty()) {
|
|
ret["gps-offset-ns"]=nullptr;
|
|
}
|
|
else {
|
|
int sv = gpsgststats.begin()->second; // freshest SV
|
|
long shift = g_svstats[{2,sv}].a0g * (1L<<16) + g_svstats[{2,sv}].a1g * gpsgststats.begin()->first; // in 2^-51 seconds units
|
|
|
|
ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21);
|
|
}
|
|
|
|
if(gpsutcstats.empty()) {
|
|
ret["gps-utc-offset-ns"]=nullptr;
|
|
}
|
|
else {
|
|
int sv = gpsutcstats.begin()->second; // freshest SV
|
|
long shift = g_svstats[{0,sv}].a0 * (1LL<<20) + g_svstats[{0,sv}].a1 * gpsutcstats.begin()->first; // In 2^-50 seconds units
|
|
|
|
ret["gps-utc-offset-ns"] = 1.073741824*ldexp(shift, -20);
|
|
}
|
|
|
|
|
|
|
|
return ret;
|
|
});
|
|
|
|
h2s.addHandler("/almanac", [](auto handler, auto req) {
|
|
nlohmann::json ret = nlohmann::json::object();
|
|
for(const auto& ae : g_beidoualma) {
|
|
nlohmann::json item = nlohmann::json::object();
|
|
if(ae.second.alma.getT0e() > 7*86400)
|
|
continue;
|
|
Point sat;
|
|
getCoordinates(0, latestTow(3), ae.second.alma, &sat);
|
|
item["eph-ecefX"]= sat.x/1000;
|
|
item["eph-ecefY"]= sat.y/1000;
|
|
item["eph-ecefZ"]= sat.z/1000;
|
|
|
|
auto longlat = getLongLat(sat.x, sat.y, sat.z);
|
|
item["eph-longitude"] = 180*longlat.first/M_PI;
|
|
item["eph-latitude"]= 180*longlat.second/M_PI;
|
|
item["t0e"] = ae.second.alma.getT0e();
|
|
item["t"]= ephAge(ae.second.alma.getT0e(), latestTow(3))/86400.0;
|
|
if(ephAge(ae.second.alma.getT0e(), latestTow(3)) < 0) {
|
|
auto match = g_tles.getBestMatch(nanoTime(3, latestWN(3), latestTow(3))/1000000000.0,
|
|
sat.x, sat.y, sat.z);
|
|
|
|
if(match.distance < 200000) {
|
|
item["best-tle"] = match.name;
|
|
item["best-tle-norad"] = match.norad;
|
|
item["best-tle-int-desig"] = match.internat;
|
|
item["best-tle-dist"] = match.distance/1000.0;
|
|
|
|
item["tle-ecefX"] = match.ecefX/1000;
|
|
item["tle-ecefY"] = match.ecefY/1000;
|
|
item["tle-ecefZ"] = match.ecefZ/1000;
|
|
|
|
item["tle-eciX"] = match.eciX/1000;
|
|
item["tle-eciY"] = match.eciY/1000;
|
|
item["tle-eciZ"] = match.eciZ/1000;
|
|
|
|
item["tle-latitude"] = 180*match.latitude/M_PI;
|
|
item["tle-longitude"] = 180*match.longitude/M_PI;
|
|
item["tle-altitude"] = match.altitude;
|
|
}
|
|
}
|
|
|
|
ret[fmt::sprintf("C%02d", ae.first)] = item;
|
|
}
|
|
|
|
for(const auto& ae : g_glonassalma) {
|
|
|
|
nlohmann::json item = nlohmann::json::object();
|
|
|
|
// ae.second.first -> even ae.second.sceond -> odd
|
|
|
|
item["e"] = ae.second.first.getE();
|
|
item["inclination"] = 180 * ae.second.first.getI0() /M_PI;
|
|
item["health"] = ae.second.first.CnA;
|
|
item["tlambdana"] = ae.second.second.gettLambdaNa();
|
|
item["lambdana"] = ae.second.second.getLambdaNaDeg();
|
|
item["hna"] = ae.second.second.hna;
|
|
|
|
ret[fmt::sprintf("R%02d", ae.first)] = item;
|
|
}
|
|
|
|
return ret;
|
|
});
|
|
|
|
h2s.addHandler("/svs", [](auto handler, auto req) {
|
|
nlohmann::json ret = nlohmann::json::object();
|
|
|
|
auto hzCorrection = getHzCorrection(time(0));
|
|
|
|
for(const auto& s: g_svstats) {
|
|
nlohmann::json item = nlohmann::json::object();
|
|
if(!s.second.tow) // I know, I know, will suck briefly
|
|
continue;
|
|
|
|
item["gnssid"] = s.first.first;
|
|
item["svid"] = s.first.second;
|
|
// perhaps check oldBeidouMessage for sow >=0 as 'completeIOD'?
|
|
|
|
|
|
if(s.first.first == 3) { // beidou
|
|
item["sisa"]=humanUra(s.second.ura);
|
|
if(s.second.t0eMSB >= 0 && s.second.t0eLSB >=0)
|
|
item["eph-age-m"] = ephAge(s.second.tow, 8.0*((s.second.t0eMSB<<15) + s.second.t0eLSB))/60.0;
|
|
|
|
if(time(0) - s.second.deltaHz.first < 60) {
|
|
item["delta_hz"] = s.second.deltaHz.second;
|
|
if(hzCorrection)
|
|
item["delta_hz_corr"] = s.second.deltaHz.second - (1561.098/1575.42)* (*hzCorrection);
|
|
}
|
|
if(s.second.tleMatch.distance >=0) {
|
|
item["best-tle"] = s.second.tleMatch.name;
|
|
item["best-tle-dist"] = s.second.tleMatch.distance /1000.0;
|
|
item["best-tle-norad"] = s.second.tleMatch.norad;
|
|
item["best-tle-int-desig"] = s.second.tleMatch.internat;
|
|
|
|
}
|
|
}
|
|
else if(s.first.first == 6) { // glonass
|
|
if(s.second.glonassMessage.FT < 16)
|
|
item["sisa"] = humanFt(s.second.glonassMessage.FT);
|
|
item["aode"] = s.second.aode;
|
|
item["iod"] = s.second.glonassMessage.Tb;
|
|
if(s.second.tleMatch.distance >=0) {
|
|
item["best-tle"] = s.second.tleMatch.name;
|
|
item["best-tle-dist"] = s.second.tleMatch.distance /1000.0;
|
|
item["best-tle-norad"] = s.second.tleMatch.norad;
|
|
item["best-tle-int-desig"] = s.second.tleMatch.internat;
|
|
}
|
|
|
|
}
|
|
if(s.second.completeIOD()) {
|
|
item["iod"]=s.second.getIOD();
|
|
if(s.first.first == 0 || s.first.first == 3) {
|
|
item["sisa"]=humanUra(s.second.ura);
|
|
// cout<<"Asked to convert "<<s.second.ura<<" for sv "<<s.first.first<<","<<s.first.second<<endl;
|
|
}
|
|
else
|
|
item["sisa"]=humanSisa(s.second.liveIOD().sisa);
|
|
item["eph-age-m"] = ephAge(s.second.tow, s.second.liveIOD().t0e)/60.0;
|
|
item["af0"] = s.second.liveIOD().af0;
|
|
item["af1"] = s.second.liveIOD().af1;
|
|
item["af2"] = (int)s.second.liveIOD().af2;
|
|
item["t0c"] = s.second.liveIOD().t0c;
|
|
|
|
Point our = g_ourpos;
|
|
Point p;
|
|
Point core;
|
|
|
|
// this should actually use local time!
|
|
getCoordinates(0, s.second.tow, s.second.liveIOD(), &p);
|
|
auto match = g_tles.getBestMatch(
|
|
s.first.first ?
|
|
utcFromGST((int)s.second.wn, (int)s.second.tow) : utcFromGPS(s.second.wn, s.second.tow),
|
|
p.x, p.y, p.z);
|
|
|
|
item["best-tle"] = match.name;
|
|
item["best-tle-norad"] = match.norad;
|
|
item["best-tle-int-desig"] = match.internat;
|
|
item["best-tle-dist"] = match.distance/1000.0;
|
|
|
|
Vector core2us(core, our);
|
|
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
|
|
|
|
double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
|
|
double deg = 180.0* (elev/M_PI);
|
|
item["elev"] = 90 - deg;
|
|
|
|
|
|
item["x"]=p.x;
|
|
item["y"]=p.y;
|
|
item["z"]=p.z;
|
|
|
|
if(time(0) - s.second.deltaHz.first < 60) {
|
|
item["delta_hz"] = s.second.deltaHz.second;
|
|
if(hzCorrection)
|
|
item["delta_hz_corr"] = s.second.deltaHz.second - *hzCorrection;
|
|
}
|
|
}
|
|
|
|
item["a0"]=s.second.a0;
|
|
item["a1"]=s.second.a1;
|
|
item["dtLS"]=s.second.dtLS;
|
|
|
|
if(s.first.first == 3) { // beidou
|
|
item["a0g"]=s.second.a0g;
|
|
item["a1g"]=s.second.a1g;
|
|
if(s.second.aode >= 0)
|
|
item["aode"]=s.second.aode;
|
|
if(s.second.aodc >= 0)
|
|
item["aodc"]=s.second.aodc;
|
|
|
|
}
|
|
|
|
|
|
if(s.first.first == 2) { // galileo
|
|
item["a0g"]=s.second.a0g;
|
|
item["a1g"]=s.second.a1g;
|
|
vector<string> options{"ok", "out of service", "will be out of service", "test"};
|
|
item["health"] =
|
|
options[s.second.e1bhs] +"/" +
|
|
options[s.second.e5bhs] +"/" +
|
|
(s.second.e1bdvs ? "no guarantee" : "val") +"/"+
|
|
(s.second.e5bdvs ? "no guarantee" : "val");
|
|
item["e5bdvs"]=s.second.e5bdvs;
|
|
item["e1bdvs"]=s.second.e1bdvs;
|
|
item["e5bhs"]=s.second.e5bhs;
|
|
item["e1bhs"]=s.second.e1bhs;
|
|
item["healthissue"]=0;
|
|
if(s.second.e1bhs == 2 || s.second.e5bhs == 2)
|
|
item["healthissue"] = 1;
|
|
if(s.second.e1bhs == 3 || s.second.e5bhs == 3)
|
|
item["healthissue"] = 1;
|
|
if(s.second.e1bdvs || s.second.e5bdvs || s.second.e1bhs == 1 || s.second.e5bhs == 1)
|
|
item["healthissue"] = 2;
|
|
|
|
}
|
|
else if(s.first.first == 0 || s.first.first == 3 || s.first.first == 6) {// gps or beidou or GLONASS
|
|
item["health"]= s.second.gpshealth ? ("NOT OK: "+to_string(s.second.gpshealth)) : string("OK");
|
|
item["healthissue"]= 2* !!s.second.gpshealth;
|
|
}
|
|
|
|
nlohmann::json perrecv = nlohmann::json::object();
|
|
for(const auto& pr : s.second.perrecv) {
|
|
if(pr.second.el > 0 && pr.second.el <= 90) {
|
|
nlohmann::json det = nlohmann::json::object();
|
|
det["elev"] = pr.second.el;
|
|
det["db"] = pr.second.db;
|
|
det["last-seen-s"] = time(0) - pr.second.t;
|
|
perrecv[to_string(pr.first)]=det;
|
|
}
|
|
}
|
|
item["perrecv"]=perrecv;
|
|
|
|
item["last-seen-s"] = time(0) - nanoTime(s.first.first, s.second.wn, s.second.tow)/1000000000;
|
|
if(s.second.latestDisco >=0) {
|
|
item["latest-disco"]= s.second.latestDisco;
|
|
}
|
|
if(s.second.timeDisco > -100 && s.second.timeDisco < 100) {
|
|
item["time-disco"]= s.second.timeDisco;
|
|
}
|
|
|
|
|
|
item["wn"] = s.second.wn;
|
|
item["tow"] = s.second.tow;
|
|
ret[fmt::sprintf("%c%02d", getGNSSChar(s.first.first), s.first.second)] = item;
|
|
}
|
|
return ret;
|
|
});
|
|
h2s.addDirectory("/", argc > 2 ? argv[2] : "./html/");
|
|
|
|
int port = argc > 1 ? atoi(argv[1]) : 29599;
|
|
std::thread ws([&h2s, port]() {
|
|
auto actx = h2s.addContext();
|
|
h2s.addListener(ComboAddress("::", port), actx);
|
|
cout<<"Listening on port "<< port <<endl;
|
|
h2s.runLoop();
|
|
});
|
|
ws.detach();
|
|
|
|
ofstream dopplercsv("doppler."+to_string(getpid())+".csv");
|
|
dopplercsv<<"timestamp gnssid sv prmes cpmes doppler preddop distance radvel locktimems iod_age prstd cpstd dostd"<<endl;
|
|
|
|
|
|
try {
|
|
for(;;) {
|
|
char bert[4];
|
|
if(readn2(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') {
|
|
cerr<<"EOF or bad magic"<<endl;
|
|
break;
|
|
}
|
|
|
|
uint16_t len;
|
|
if(readn2(0, &len, 2) != 2)
|
|
break;
|
|
len = htons(len);
|
|
char buffer[len];
|
|
if(readn2(0, buffer, len) != len)
|
|
break;
|
|
|
|
NavMonMessage nmm;
|
|
nmm.ParseFromString(string(buffer, len));
|
|
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
|
int gnssid = nmm.rd().gnssid();
|
|
int sv = nmm.rd().gnsssv();
|
|
pair<int,int> id{gnssid, sv};
|
|
g_svstats[id].perrecv[nmm.sourceid()].db = nmm.rd().db();
|
|
g_svstats[id].perrecv[nmm.sourceid()].el = nmm.rd().el();
|
|
g_svstats[id].perrecv[nmm.sourceid()].azi = nmm.rd().azi();
|
|
|
|
// THIS HAS TO SPLIT OUT PER SOURCE
|
|
idb.addValue(id, "db", nmm.rd().db());
|
|
if(nmm.rd().el() <= 90 && nmm.rd().el() > 0)
|
|
idb.addValue(id, "elev", nmm.rd().el());
|
|
idb.addValue(id, "azi", nmm.rd().azi());
|
|
}
|
|
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
|
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
|
int sv = nmm.gi().gnsssv();
|
|
pair<int, int> id={2,sv};
|
|
g_svstats[id].wn = nmm.gi().gnsswn();
|
|
auto& svstat = g_svstats[id];
|
|
auto oldgm = svstat.galmsg;
|
|
|
|
unsigned int wtype = svstat.galmsg.parse(inav);
|
|
if(wtype == 1 || wtype == 2 || wtype == 3) {
|
|
idb.addValue(id, "iod-live", svstat.galmsg.iodnav);
|
|
}
|
|
if(1) {
|
|
// cout<<sv <<"\t" << wtype << "\t" << nmm.gi().gnsstow() << "\t"<< nmm.sourceid() << endl;
|
|
/* if(g_svstats[id].tow > nmm.gi().gnsstow()) {
|
|
cout<<" wtype "<<wtype<<", was about to set tow backwards for "<<sv<<", "<<g_svstats[id].tow << " > "<<nmm.gi().gnsstow()<<", " << ((signed)g_svstats[id].tow - (signed)nmm.gi().gnsstow()) << ", source "<<nmm.sourceid()<<endl;
|
|
}*/
|
|
}
|
|
g_svstats[id].tow = nmm.gi().gnsstow();
|
|
|
|
// g_svstats[id].perrecv[nmm.sourceid()].wn = nmm.gi().gnsswn();
|
|
// g_svstats[id].perrecv[nmm.sourceid()].tow = nmm.gi().gnsstow();
|
|
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
|
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
|
|
// for(auto& c : inav)
|
|
// fmt::printf("%02x ", c);
|
|
// cout<<"About to add word for galileo sv "<<id.first<<","<<id.second<<": word = "<<wtype<<endl;
|
|
g_svstats[id].addGalileoWord(inav);
|
|
if(g_svstats[id].e1bhs || g_svstats[id].e5bhs || g_svstats[id].e1bdvs || g_svstats[id].e5bdvs) {
|
|
if(sv != 18 && sv != 14)
|
|
cout << "sv "<<sv<<" health: " << g_svstats[id].e1bhs <<" " << g_svstats[id].e5bhs <<" " << g_svstats[id].e1bdvs <<" "<< g_svstats[id].e5bdvs <<endl;
|
|
}
|
|
|
|
if(wtype >=1 && wtype <= 4) { // ephemeris
|
|
uint16_t iod = getbitu(&inav[0], 6, 10);
|
|
if(wtype == 3) {
|
|
idb.addValue(id, "sisa", g_svstats[id].iods[iod].sisa);
|
|
}
|
|
else if(wtype == 4) {
|
|
|
|
idb.addValue(id, "af0", g_svstats[id].iods[iod].af0);
|
|
idb.addValue(id, "af1", g_svstats[id].iods[iod].af1);
|
|
idb.addValue(id, "af2", g_svstats[id].iods[iod].af2);
|
|
idb.addValue(id, "t0c", g_svstats[id].iods[iod].t0c * 60);
|
|
|
|
double age = ephAge(g_svstats[id].tow, g_svstats[id].iods[iod].t0c * 60);
|
|
|
|
double offset = ldexp(1000.0*(1.0*g_svstats[id].iods[iod].af0 + ldexp(age*g_svstats[id].iods[iod].af1, -12)), -34);
|
|
idb.addValue(id, "atomic_offset_ns", 1000000.0*offset);
|
|
|
|
if(oldgm.af0 && oldgm.t0c != svstat.galmsg.t0c) {
|
|
auto oldOffset = oldgm.getAtomicOffset(svstat.tow);
|
|
auto newOffset = svstat.galmsg.getAtomicOffset(svstat.tow);
|
|
svstat.timeDisco = oldOffset.first - newOffset.first;
|
|
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
|
}
|
|
|
|
}
|
|
else
|
|
;
|
|
}
|
|
else if(wtype == 5) {
|
|
idb.addValue(id, "ai0", g_svstats[id].ai0);
|
|
idb.addValue(id, "ai1", g_svstats[id].ai1);
|
|
idb.addValue(id, "ai2", g_svstats[id].ai2);
|
|
|
|
idb.addValue(id, "sf1", g_svstats[id].sf1);
|
|
idb.addValue(id, "sf2", g_svstats[id].sf2);
|
|
idb.addValue(id, "sf3", g_svstats[id].sf3);
|
|
idb.addValue(id, "sf4", g_svstats[id].sf4);
|
|
idb.addValue(id, "sf5", g_svstats[id].sf5);
|
|
|
|
idb.addValue(id, "BGDE1E5a", g_svstats[id].BGDE1E5a);
|
|
idb.addValue(id, "BGDE1E5b", g_svstats[id].BGDE1E5b);
|
|
|
|
idb.addValue(id, "e1bhs", g_svstats[id].e1bhs);
|
|
idb.addValue(id, "e5bhs", g_svstats[id].e5bhs);
|
|
idb.addValue(id, "e5bdvs", g_svstats[id].e5bdvs);
|
|
idb.addValue(id, "e1bdvs", g_svstats[id].e1bdvs);
|
|
}
|
|
else if(wtype == 6) { // GST-UTC
|
|
idb.addValue(id, "a0", g_svstats[id].a0);
|
|
idb.addValue(id, "a1", g_svstats[id].a1);
|
|
int dw = (uint8_t)g_svstats[id].wn - g_svstats[id].wn0t;
|
|
int age = dw * 7 * 86400 + g_svstats[id].tow - g_svstats[id].t0t; // t0t is PRESCALED
|
|
|
|
long shift = g_svstats[id].a0 * (1LL<<20) + g_svstats[id].a1 * age; // in 2^-50 seconds units
|
|
idb.addValue(id, "utc_diff_ns", 1.073741824*ldexp(1.0*shift, -20));
|
|
|
|
g_dtLS = g_svstats[id].dtLS;
|
|
}
|
|
else if(wtype == 10) { // GSTT GPS
|
|
idb.addValue(id, "a0g", g_svstats[id].a0g);
|
|
idb.addValue(id, "a1g", g_svstats[id].a1g);
|
|
idb.addValue(id, "t0g", g_svstats[id].t0g);
|
|
uint8_t wn0g = g_svstats[id].wn0t;
|
|
int dwg = (((uint8_t)g_svstats[id].wn)&(1+2+4+8+16+32)) - wn0g;
|
|
int age = dwg*7*86400 + g_svstats[id].tow - g_svstats[id].t0g * 3600;
|
|
|
|
long shift = g_svstats[id].a0g * (1L<<16) + g_svstats[id].a1g * age; // in 2^-51 seconds units
|
|
|
|
idb.addValue(id, "gps_gst_offset_ns", 1.073741824*ldexp(shift, -21));
|
|
|
|
|
|
}
|
|
|
|
for(auto& ent : g_svstats) {
|
|
// fmt::printf("%2d\t", ent.first);
|
|
id=ent.first;
|
|
if(ent.second.completeIOD() && ent.second.prevIOD.first >= 0) {
|
|
// time_t t = utcFromGST((int)ent.second.wn, (int)ent.second.tow);
|
|
// cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n";
|
|
|
|
// double clockage = ephAge(ent.second.tow, ent.second.liveIOD().t0c * 60);
|
|
// double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46);
|
|
|
|
int ephage = ephAge(ent.second.tow, ent.second.prevIOD.second.t0e);
|
|
if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) {
|
|
|
|
cout<<humanTime(ent.second.wn, ent.second.tow)<<" gnssid "<<ent.first.first<<" sv "<<ent.first.second<<" changed sisa from "<<(unsigned int) ent.second.prevIOD.second.sisa<<" ("<<
|
|
humanSisa(ent.second.prevIOD.second.sisa)<<") to " << (unsigned int)ent.second.liveIOD().sisa << " ("<<
|
|
humanSisa(ent.second.liveIOD().sisa)<<"), lastseen = "<< (ephage/3600.0) <<"h"<<endl;
|
|
}
|
|
|
|
Point p, oldp;
|
|
getCoordinates(ent.second.wn, ent.second.tow, ent.second.liveIOD(), &p);
|
|
// cout << ent.first << ": iod= "<<ent.second.getIOD()<<" "<< p.x/1000.0 << ", "<< p.y/1000.0 <<", "<<p.z/1000.0<<endl;
|
|
|
|
// cout<<"OLD: \n";
|
|
getCoordinates(ent.second.wn, ent.second.tow, ent.second.prevIOD.second, &oldp);
|
|
// cout << ent.first << ": iod= "<<ent.second.prevIOD.first<<" "<< oldp.x/1000.0 << ", "<< oldp.y/1000.0 <<", "<<oldp.z/1000.0<<endl;
|
|
|
|
if(ent.second.prevIOD.second.t0e < ent.second.liveIOD().t0e) {
|
|
double hours = ((ent.second.liveIOD().t0e - ent.second.prevIOD.second.t0e)/3600.0);
|
|
double disco = Vector(p, oldp).length();
|
|
cout<<id.first<<","<<id.second<<" discontinuity after "<< hours<<" hours: "<< disco <<endl;
|
|
idb.addValue(id, "iod-actual", ent.second.getIOD());
|
|
idb.addValue(id, "iod-hours", hours);
|
|
|
|
|
|
if(hours < 4) {
|
|
idb.addValue(id, "eph-disco", disco);
|
|
g_svstats[id].latestDisco= disco;
|
|
}
|
|
else
|
|
g_svstats[id].latestDisco= -1;
|
|
|
|
|
|
|
|
if(0 && hours < 2) {
|
|
ofstream orbitcsv("orbit."+to_string(id.first)+"."+to_string(id.second)+"."+to_string(ent.second.prevIOD.first)+"-"+to_string(ent.second.getIOD())+".csv");
|
|
|
|
orbitcsv << "timestamp x y z oldx oldy oldz\n";
|
|
orbitcsv << fixed;
|
|
for(int offset = -7200; offset < 7200; offset += 30) {
|
|
int t = ent.second.liveIOD().t0e + offset;
|
|
Point p, oldp;
|
|
getCoordinates(ent.second.wn, t, ent.second.liveIOD(), &p);
|
|
getCoordinates(ent.second.wn, t, ent.second.prevIOD.second, &oldp);
|
|
time_t posix = utcFromGST(ent.second.wn, t);
|
|
orbitcsv << posix <<" "
|
|
<<p.x<<" " <<p.y<<" "<<p.z<<" "
|
|
<<oldp.x<<" " <<oldp.y<<" "<<oldp.z<<"\n";
|
|
}
|
|
}
|
|
}
|
|
ent.second.clearPrev();
|
|
}
|
|
}
|
|
}
|
|
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
|
// XXX!! this has to deal with source id!
|
|
g_ourpos.x = nmm.op().x();
|
|
g_ourpos.y = nmm.op().y();
|
|
g_ourpos.z = nmm.op().z();
|
|
}
|
|
else if(nmm.type() == NavMonMessage::RFDataType) {
|
|
int sv = nmm.rfd().gnsssv();
|
|
pair<int,int> id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()};
|
|
if(id.first == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
|
|
auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].oldBeidouMessage, 1561.098 * 1000000);
|
|
Point p;
|
|
getCoordinates(0, nmm.rfd().rcvtow(), g_svstats[id].oldBeidouMessage, &p);
|
|
|
|
if(isnan(res.preddop)) {
|
|
cerr<<"Problem with doppler calculation for C"<<id.second<<": "<<endl;
|
|
exit(1);
|
|
}
|
|
|
|
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
|
dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << res.preddop << " " << Vector(g_ourpos, res.sat).length() << " " <<res.radvel <<" " << nmm.rfd().locktimems()<<" " <<res.ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
|
nmm.rfd().dostd() << " "<<nmm.rfd().rcvtow()<<endl;
|
|
|
|
if(t - g_svstats[id].deltaHz.first > 10) {
|
|
|
|
g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop};
|
|
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
|
auto corr = getHzCorrection(t);
|
|
if(corr) {
|
|
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - (1561.098/1575.42) * (*corr));
|
|
}
|
|
}
|
|
|
|
}
|
|
else if(g_svstats[id].completeIOD()) {
|
|
auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].liveIOD(), 1575.42 * 1000000);
|
|
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
|
dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << res.preddop << " " << Vector(g_ourpos, res.sat).length() << " " <<res.radvel <<" " << nmm.rfd().locktimems()<<" " <<res.ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
|
nmm.rfd().dostd() << endl;
|
|
|
|
if(t - g_svstats[id].deltaHz.first > 10) {
|
|
g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop};
|
|
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
|
auto corr = getHzCorrection(t);
|
|
if(corr) {
|
|
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - *corr);
|
|
}
|
|
}
|
|
|
|
// cout<<"Had doppler for "<<id.first<<", "<<id.second<<endl;
|
|
}
|
|
}
|
|
else if(nmm.type()== NavMonMessage::GPSInavType) {
|
|
auto cond = getCondensedGPSMessage(std::basic_string<uint8_t>((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
|
pair<int,int> id{nmm.gpsi().gnssid(), nmm.gpsi().gnsssv()};
|
|
|
|
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
|
|
|
auto& svstat = g_svstats[id];
|
|
auto oldsvstat = svstat;
|
|
uint8_t page;
|
|
int frame=parseGPSMessage(cond, svstat, &page);
|
|
if(frame == 1) {
|
|
idb.addValue(id, "af0", 8* svstat.af0); // scaled to galileo units - native gps: 2^-31
|
|
idb.addValue(id, "af1", 8* svstat.af1); // scaled to galileo units - native gps: 2^-43
|
|
idb.addValue(id, "af2", 16* svstat.af2); // scaled to galileo units
|
|
idb.addValue(id, "t0c", 16 * svstat.t0c);
|
|
// cout<<"Got ura "<<svstat.ura<<" for sv "<<id.first<<","<<id.second<<endl;
|
|
idb.addValue(id, "ura", svstat.ura);
|
|
|
|
double age = ephAge(g_svstats[id].tow, g_svstats[id].t0c * 16);
|
|
|
|
double offset = ldexp(1000.0*(1.0*g_svstats[id].af0 + ldexp(age*g_svstats[id].af1, -12)), -31);
|
|
idb.addValue(id, "atomic_offset_ns", 1000000.0*offset);
|
|
|
|
if(oldsvstat.af0 && oldsvstat.t0c != svstat.t0c) {
|
|
auto oldOffset = getAtomicOffset(svstat.tow, oldsvstat);
|
|
auto newOffset = getAtomicOffset(svstat.tow, svstat);
|
|
svstat.timeDisco = oldOffset.first - newOffset.first;
|
|
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
|
}
|
|
}
|
|
else if(frame==2) {
|
|
idb.addValue(id, "gpshealth", g_svstats[id].gpshealth);
|
|
}
|
|
else if(frame==4 && page==18) {
|
|
idb.addValue(id, "a0", g_svstats[id].a0);
|
|
idb.addValue(id, "a1", g_svstats[id].a1);
|
|
int dw = (uint8_t)g_svstats[id].wn - g_svstats[id].wn0t;
|
|
int age = dw * 7 * 86400 + g_svstats[id].tow - g_svstats[id].t0t; // t0t is PRESCALED
|
|
|
|
long shift = g_svstats[id].a0 * (1LL<<20) + g_svstats[id].a1 * age; // in 2^-50 seconds units
|
|
idb.addValue(id, "utc_diff_ns", 1.073741824*ldexp(1.0*shift, -20));
|
|
}
|
|
|
|
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
|
g_svstats[id].tow = nmm.gpsi().gnsstow();
|
|
g_svstats[id].wn = nmm.gpsi().gnsswn();
|
|
if(g_svstats[id].wn < 512)
|
|
g_svstats[id].wn += 2048;
|
|
}
|
|
else if(nmm.type()== NavMonMessage::BeidouInavTypeD1) {
|
|
pair<int,int> id{nmm.bid1().gnssid(), nmm.bid1().gnsssv()};
|
|
|
|
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
|
|
|
auto& svstat = g_svstats[id];
|
|
uint8_t pageno;
|
|
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
|
auto& bm = svstat.beidouMessage;
|
|
|
|
int fraid=bm.parse(cond, &pageno);
|
|
svstat.tow = nmm.bid1().gnsstow();
|
|
svstat.wn = nmm.bid1().gnsswn();
|
|
if(fraid == 1) {
|
|
svstat.ura = bm.urai;
|
|
svstat.gpshealth = bm.sath1;
|
|
svstat.af0 = bm.a0;
|
|
svstat.af1 = bm.a1;
|
|
svstat.af2 = bm.a2;
|
|
svstat.aode = bm.aode;
|
|
svstat.aodc = bm.aodc;
|
|
idb.addValue(id, "atomic_offset_ns", 1000000.0*bm.getAtomicOffset().first);
|
|
idb.addValue(id, "t0c", bm.getT0c());
|
|
idb.addValue(id, "af0", bm.a0 * 2);
|
|
idb.addValue(id, "af1", bm.a1 / 16);
|
|
idb.addValue(id, "af2", bm.a2 / 128); // scaled to galileo units
|
|
if(svstat.lastBeidouMessage1.wn >=0 && svstat.lastBeidouMessage1.t0c != bm.t0c) {
|
|
auto oldOffset = svstat.lastBeidouMessage1.getAtomicOffset(bm.sow);
|
|
auto newOffset = bm.getAtomicOffset(bm.sow);
|
|
svstat.timeDisco = oldOffset.first - newOffset.first;
|
|
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
|
}
|
|
svstat.lastBeidouMessage1 = bm;
|
|
}
|
|
if(fraid == 2) {
|
|
svstat.lastBeidouMessage2 = bm;
|
|
svstat.t0eMSB = bm.t0eMSB;
|
|
}
|
|
if(fraid == 3) {
|
|
svstat.t0eLSB = bm.t0eLSB;
|
|
Point oldpoint, newpoint;
|
|
if(bm.sow - svstat.lastBeidouMessage2.sow == 6 && svstat.oldBeidouMessage.sow >= 0) {
|
|
getCoordinates(svstat.wn, svstat.tow, bm, &newpoint);
|
|
|
|
auto match = g_tles.getBestMatch(nanoTime(3, svstat.wn, svstat.tow)/1000000000.0, newpoint.x, newpoint.y, newpoint.z);
|
|
svstat.tleMatch = match;
|
|
|
|
if(svstat.oldBeidouMessage.getT0e() != svstat.beidouMessage.getT0e()) {
|
|
getCoordinates(svstat.wn, svstat.tow, svstat.oldBeidouMessage, &oldpoint);
|
|
Vector jump(oldpoint ,newpoint);
|
|
cout<<fmt::sprintf("Discontinuity C%02d (%f,%f,%f) -> (%f, %f, %f), jump: %f, seconds: %f\n",
|
|
id.second, oldpoint.x, oldpoint.y, oldpoint.z,
|
|
newpoint.x, newpoint.y, newpoint.z, jump.length(), (double)bm.getT0e() - svstat.oldBeidouMessage.getT0e());
|
|
double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600;
|
|
if(hours < 4) {
|
|
svstat.latestDisco = jump.length();
|
|
idb.addValue(id, "eph-disco", jump.length());
|
|
}
|
|
else
|
|
svstat.latestDisco = -1;
|
|
}
|
|
|
|
}
|
|
if(bm.sqrtA) // only copy in if complete
|
|
svstat.oldBeidouMessage = bm;
|
|
}
|
|
else if((fraid == 4 && 1<= pageno && pageno <= 24) ||
|
|
(fraid == 5 && 1<= pageno && pageno <= 6) ||
|
|
(fraid == 5 && 11<= pageno && pageno <= 23) ) {
|
|
|
|
struct BeidouAlmanacEntry bae;
|
|
// bm.alma.AmEpID = svstat.oldBeidouMessage.alma.AmEpID; // this comes from older messages
|
|
|
|
if(processBeidouAlmanac(bm, bae)) {
|
|
g_beidoualma[bae.sv]=bae;
|
|
}
|
|
}
|
|
|
|
if(fraid==5 && pageno == 9) {
|
|
svstat.a0g = bm.a0gps;
|
|
svstat.a1g = bm.a1gps;
|
|
}
|
|
|
|
if(fraid==5 && pageno == 10) {
|
|
svstat.a0 = bm.a0utc;
|
|
svstat.a1 = bm.a1utc;
|
|
g_dtLSBeidou = bm.deltaTLS;
|
|
// cout<<"Beidou leap seconds: "<<g_dtLSBeidou<<endl;
|
|
}
|
|
// Point core, sat;
|
|
|
|
// getCoordinates(svstat.wn, svstat.tow, bm, &sat);
|
|
// Vector l(core, sat);
|
|
// cout<<"C"<<id.second<< " "<<bm.sow<<" "<<(bm.sow % 30 )<<" FraID "<<fraid<<" "<<fmt::format("({0}, {1}, {2})", sat.x, sat.y, sat.z) <<", r: "<<l.length()<<" elev: "<<getElevation(sat)<<endl;
|
|
}
|
|
else if(nmm.type()== NavMonMessage::BeidouInavTypeD2) {
|
|
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
|
/*
|
|
int fraid = getbitu(&cond[0], beidouBitconv(16), 3);
|
|
int sow = getbitu(&cond[0], beidouBitconv(19), 20);
|
|
int pnum = getbitu(&cond[0], beidouBitconv(43), 4);
|
|
int pre = getbitu(&cond[0], beidouBitconv(1), 11);
|
|
|
|
// cout<<"C"<< nmm.bid2().gnsssv() << " sent D2 message, pre "<<pre<<" sow "<<sow<<", FraID "<<fraid;
|
|
// if(fraid == 1)
|
|
// cout <<" pnum "<<pnum;
|
|
// cout<<endl;
|
|
*/
|
|
}
|
|
else if(nmm.type()== NavMonMessage::GlonassInavType) {
|
|
pair<int,int> id{nmm.gloi().gnssid(), nmm.gloi().gnsssv()};
|
|
auto& svstat = g_svstats[id];
|
|
|
|
auto& gm = svstat.glonassMessage;
|
|
auto oldgm = gm;
|
|
int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
|
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
|
if(strno == 1 && gm.n4 != 0 && gm.NT !=0) {
|
|
uint32_t glotime = gm.getGloTime(); // this starts GLONASS time at 31st of december 1995, 00:00 UTC
|
|
svstat.wn = glotime / (7*86400);
|
|
svstat.tow = glotime % (7*86400);
|
|
}
|
|
else if(strno == 2) {
|
|
svstat.gpshealth = gm.Bn;
|
|
}
|
|
else if(strno == 4) {
|
|
svstat.aode = gm.En * 24;
|
|
idb.addValue(id, "glo_taun_ns", gm.getTaunNS());
|
|
idb.addValue(id, "ft", gm.FT);
|
|
if(oldgm.taun && oldgm.taun != gm.taun) {
|
|
if(gm.getGloTime() - oldgm.getGloTime() < 300) {
|
|
svstat.timeDisco = gm.getTaunNS() - oldgm.getTaunNS();
|
|
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
|
}
|
|
}
|
|
if(gm.x && gm.y && gm.z) {
|
|
time_t now = nmm.localutcseconds() + 3*3600;
|
|
struct tm tm;
|
|
memset(&tm, 0, sizeof(tm));
|
|
gmtime_r(&now, &tm);
|
|
tm.tm_hour = (gm.Tb/4.0);
|
|
tm.tm_min = (gm.Tb % 4)*15;
|
|
tm.tm_sec = 0;
|
|
|
|
auto match = g_tles.getBestMatch(timegm(&tm)-3*3600, gm.getX(), gm.getY(), gm.getZ());
|
|
svstat.tleMatch = match;
|
|
}
|
|
}
|
|
else if(strno == 6 || strno == 8 || strno == 10 || strno == 12 || strno == 14) {
|
|
svstat.glonassAlmaEven = {nmm.localutcseconds(), gm};
|
|
}
|
|
else if(strno == 7 || strno == 9 || strno == 11 || strno == 13 || strno == 15) {
|
|
if(nmm.localutcseconds() - svstat.glonassAlmaEven.first < 4 && svstat.glonassAlmaEven.second.strtype == gm.strtype -1) {
|
|
g_glonassalma[svstat.glonassAlmaEven.second.nA] = make_pair(svstat.glonassAlmaEven.second, gm);
|
|
}
|
|
}
|
|
// cout<<"GLONASS R"<<id.second<<" str "<<strno<<endl;
|
|
}
|
|
else {
|
|
cout<<"Unknown type "<< (int)nmm.type()<<endl;
|
|
}
|
|
|
|
}
|
|
}
|
|
catch(EofException& e)
|
|
{}
|
|
}
|
|
catch(std::exception& e)
|
|
{
|
|
cerr<<"Exiting because of fatal error "<<e.what()<<endl;
|
|
}
|