too much to mention
bert hubert
parent
286befa8a9
commit
f9424cce6c
|
|
@ -1,3 +1,6 @@
|
|||
navmon.pb.cc
|
||||
navmon.pb.h
|
||||
*.csv
|
||||
# Prerequisites
|
||||
*.d
|
||||
*~
|
||||
|
|
|
|||
6
Makefile
6
Makefile
|
|
@ -1,6 +1,6 @@
|
|||
CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/
|
||||
|
||||
PROGRAMS = navparse ubxtool navnexus navrecv
|
||||
PROGRAMS = navparse ubxtool navnexus navrecv navdump
|
||||
|
||||
all: $(PROGRAMS)
|
||||
|
||||
|
|
@ -15,6 +15,10 @@ SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simpl
|
|||
navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o
|
||||
g++ -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf # -lwslay
|
||||
|
||||
navdump: navdump.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o
|
||||
g++ -std=gnu++17 $^ -o $@ -pthread -lprotobuf
|
||||
|
||||
|
||||
navnexus: navnexus.o ext/fmt-5.2.1/src/format.o $(SIMPLESOCKETS) ubx.o bits.o navmon.pb.o storage.o
|
||||
g++ -std=gnu++17 $^ -o $@ -pthread -lprotobuf
|
||||
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ void getCoordinates(int wn, double tow, const auto& iod, Point* p, bool quiet=tr
|
|||
|
||||
double sqrtA = 1.0*iod.sqrtA / (1ULL<<19);
|
||||
double deltan = M_PI * 1.0*iod.deltan / (1LL<<43);
|
||||
double t0e = 60.0*iod.t0e;
|
||||
double t0e = iod.t0e; // t0e is PRE-SCALED
|
||||
double m0 = M_PI * 1.0*iod.m0 / (1LL<<31);
|
||||
double e = 1.0*iod.e / (1ULL<<33);
|
||||
double omega = M_PI * 1.0*iod.omega / (1LL<<31);
|
||||
|
|
@ -65,7 +65,7 @@ void getCoordinates(int wn, double tow, const auto& iod, Point* p, bool quiet=tr
|
|||
double A3 = pow(sqrtA, 6.0);
|
||||
|
||||
double n0 = sqrt(mu/A3);
|
||||
double tk = tow - t0e; // in seconds, ignores WN!!! XX!!! ! XX
|
||||
double tk = tow - t0e; // in seconds, should do ephAge
|
||||
|
||||
double n = n0 + deltan;
|
||||
if(!quiet)
|
||||
|
|
|
|||
55
gps.hh
55
gps.hh
|
|
@ -4,14 +4,14 @@
|
|||
#include <iostream>
|
||||
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
// expects input as 24 bit read to to use messages
|
||||
void parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out)
|
||||
// expects input as 24 bit read to to use messages, returns frame number
|
||||
int parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out, uint8_t* pageptr=0)
|
||||
{
|
||||
using namespace std;
|
||||
int frame = getbitu(&cond[0], 24+19, 3);
|
||||
// 10 * 4 bytes in payload now
|
||||
out.tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
|
||||
cerr << "Preamble: "<<getbitu(&cond[0], 0, 8) <<", frame: "<< frame<<", truncated TOW: "<<out.tow<<endl;
|
||||
// cerr << "Preamble: "<<getbitu(&cond[0], 0, 8) <<", frame: "<< frame<<", truncated TOW: "<<out.tow<<endl;
|
||||
if(frame == 1) {
|
||||
// word 1, word 2 are TLM and HOW
|
||||
// 2 bits of padding on each word
|
||||
|
|
@ -26,35 +26,35 @@ void parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out)
|
|||
// 1-8: LSB of IODC
|
||||
// 9-24:
|
||||
|
||||
out.wn = getbitu(&cond[0], 2*24, 10);
|
||||
out.wn = 2048 + getbitu(&cond[0], 2*24, 10);
|
||||
out.ura = getbitu(&cond[0], 2*24+12, 4);
|
||||
out.gpshealth = getbitu(&cond[0], 2*24+16, 6);
|
||||
|
||||
cerr<<"GPS Week Number: "<< out.wn <<", URA: "<< (int)out.ura<<", health: "<<
|
||||
(int)out.gpshealth <<endl;
|
||||
// cerr<<"GPS Week Number: "<< out.wn <<", URA: "<< (int)out.ura<<", health: "<<
|
||||
// (int)out.gpshealth <<endl;
|
||||
|
||||
out.af2 = getbits(&cond[0], 8*24, 8); // * 2^-55
|
||||
out.af1 = getbits(&cond[0], 8*24 + 8, 16); // * 2^-43
|
||||
out.af0 = getbits(&cond[0], 9*24, 22); // * 2^-31
|
||||
out.t0c = getbits(&cond[0], 7*24 + 8, 16); // * 16
|
||||
cerr<<"t0c*16: "<<out.t0c*16<<", af2: "<< (int)out.af2 <<", af1: "<< out.af1 <<", af0: "<<
|
||||
out.af0 <<endl;
|
||||
out.t0c = getbits(&cond[0], 7*24 + 8, 16); // * 16
|
||||
// cerr<<"t0c*16: "<<out.t0c*16<<", af2: "<< (int)out.af2 <<", af1: "<< out.af1 <<", af0: "<<
|
||||
// out.af0 <<endl;
|
||||
}
|
||||
else if(frame == 2) {
|
||||
out.iod = getbitu(&cond[0], 2*24, 8);
|
||||
auto& eph = out.getEph(out.iod);
|
||||
int iod = getbitu(&cond[0], 2*24, 8);
|
||||
auto& eph = out.getEph(iod);
|
||||
eph.words[2]=1;
|
||||
eph.t0e = getbits(&cond[0], 9*24, 16) * 16.0 /60; // XXX GALILEO ADJUSTMENT
|
||||
cerr<<"IODe "<<(int)out.iod<<", t0e "<< eph.t0e << " = "<< 16* eph.t0e <<"s"<<endl;
|
||||
eph.t0e = getbits(&cond[0], 9*24, 16) * 16.0; // WE SCALE THIS FOR THE USER!!
|
||||
// cerr<<"IODe "<<(int)iod<<", t0e "<< eph.t0e << " = "<< 16* eph.t0e <<"s"<<endl;
|
||||
|
||||
eph.e= getbitu(&cond[0], 5*24+16, 32);
|
||||
cerr<<"e: "<<ldexp(eph.e, -33)<<", ";
|
||||
// cerr<<"e: "<<ldexp(eph.e, -33)<<", ";
|
||||
|
||||
|
||||
// sqrt(A), 32 bits, 2^-19
|
||||
eph.sqrtA= getbitu(&cond[0], 7*24+ 16, 32);
|
||||
double sqrtA=ldexp(eph.sqrtA, -19); // 2^-19
|
||||
cerr<<"Radius: "<<sqrtA*sqrtA<<endl;
|
||||
// double sqrtA=ldexp(eph.sqrtA, -19); // 2^-19
|
||||
// cerr<<"Radius: "<<sqrtA*sqrtA<<endl;
|
||||
|
||||
eph.crs = getbits(&cond[0], 2*24 + 8, 16); // 2^-5 meters
|
||||
eph.deltan = getbits(&cond[0], 3*24, 16); // 2^-43 semi-circles/s
|
||||
|
|
@ -62,11 +62,11 @@ void parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out)
|
|||
|
||||
eph.cuc = getbits(&cond[0], 5*24, 16); // 2^-29 RADIANS
|
||||
eph.cus = getbits(&cond[0], 7*24, 16); // 2^-29 RADIANS
|
||||
out.checkCompleteAndClean();
|
||||
out.checkCompleteAndClean(iod);
|
||||
}
|
||||
else if(frame == 3) {
|
||||
out.iod = getbitu(&cond[0], 9*24, 8);
|
||||
auto& eph = out.getEph(out.iod);
|
||||
int iod = getbitu(&cond[0], 9*24, 8);
|
||||
auto& eph = out.getEph(iod);
|
||||
eph.words[3]=1;
|
||||
eph.cic = getbits(&cond[0], 2*24, 16); // 2^-29 RADIANS
|
||||
eph.omega0 = getbits(&cond[0], 2*24 + 16, 32); // 2^-31 semi-circles
|
||||
|
|
@ -78,24 +78,28 @@ void parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out)
|
|||
|
||||
eph.omegadot = getbits(&cond[0], 8*24, 24); // 2^-43, semi-circles/s
|
||||
eph.idot = getbits(&cond[0], 9*24+8, 14); // 2^-43, semi-cirlces/s
|
||||
out.checkCompleteAndClean();
|
||||
out.checkCompleteAndClean(iod);
|
||||
}
|
||||
else if(frame == 4) { // this is a carousel frame
|
||||
int page = getbitu(&cond[0], 2*24 + 2, 6);
|
||||
cerr<<"Frame 4, page "<<page;
|
||||
if(pageptr)
|
||||
*pageptr=0;
|
||||
// cerr<<"Frame 4, page "<<page;
|
||||
if(page == 56) { // 56 is the new 18 somehow?
|
||||
if(pageptr)
|
||||
*pageptr=18;
|
||||
out.a0 = getbits(&cond[0], 6*24 , 32); // 2^-30
|
||||
out.a1 = getbits(&cond[0], 5*24 , 24); // 2^-50
|
||||
|
||||
out.t0t = getbitu(&cond[0], 7*24 + 8, 8); // 2^12
|
||||
out.t0t = getbitu(&cond[0], 7*24 + 8, 8) * 4096; // WE SCALE THIS FOR THE USER!
|
||||
out.wn0t = getbitu(&cond[0], 7*24 + 16, 8);
|
||||
out.dtLS = getbits(&cond[0], 8*24, 8);
|
||||
out.dtLSF = getbits(&cond[0], 9*24, 8);
|
||||
|
||||
cerr<<": a0: "<<out.a0<<", a1: "<<out.a1<<", t0t: "<< out.t0t * (1<<12) <<", wn0t: "<< out.wn0t<<", rough offset: "<<ldexp(out.a0, -30)<<endl;
|
||||
cerr<<"deltaTLS: "<< (int)out.dtLS<<", post "<< (int)out.dtLSF<<endl;
|
||||
// cerr<<": a0: "<<out.a0<<", a1: "<<out.a1<<", t0t: "<< out.t0t * (1<<12) <<", wn0t: "<< out.wn0t<<", rough offset: "<<ldexp(out.a0, -30)<<endl;
|
||||
// cerr<<"deltaTLS: "<< (int)out.dtLS<<", post "<< (int)out.dtLSF<<endl;
|
||||
}
|
||||
else cerr<<endl;
|
||||
//else cerr<<endl;
|
||||
// page 18 contains UTC -> 56
|
||||
// page 25 -> 63
|
||||
// 2-10 -> 25 -> 32 ??
|
||||
|
|
@ -103,4 +107,5 @@ void parseGPSMessage(std::basic_string_view<uint8_t> cond, auto& out)
|
|||
else if(frame == 5) { // this is a caroussel frame
|
||||
// cerr<<"Frame 5, SV: "<<getbitu(&cond[0], 2*32 + 2 +2, 6)<<endl;
|
||||
}
|
||||
return frame;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ function maketable(str, arr)
|
|||
enter().
|
||||
append("tr");
|
||||
|
||||
var columns = ["sv", "iod", "eph-age-m", "sisa", "e1bhs", "e1bdvs", "e5bhs", "e5bdvs", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];
|
||||
var columns = ["sv", "iod", "eph-age-m", "latest-disco", "sisa", "e1bhs", "e1bdvs", "e5bhs", "e5bdvs", "gpshealth", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];
|
||||
|
||||
// append the header row
|
||||
thead.append("tr")
|
||||
|
|
@ -31,15 +31,28 @@ function maketable(str, arr)
|
|||
ret.column = column;
|
||||
ret.color=null;
|
||||
if(column == "eph-age-m") {
|
||||
var b = moment.duration(row[column], 'm');
|
||||
ret.value = b.humanize();
|
||||
if(row[column] != null) {
|
||||
var b = moment.duration(-row[column], 'm');
|
||||
ret.value = b.humanize(true);
|
||||
}
|
||||
else
|
||||
ret.value="";
|
||||
}
|
||||
|
||||
else if(column == "last-seen-s") {
|
||||
var b = moment.duration(row[column], 's');
|
||||
ret.value = b.humanize();
|
||||
}
|
||||
// else if(column == "elev")
|
||||
// ret.value = row[column].toFixed(1);
|
||||
else if(column == "gpshealth" && row[column] != null) {
|
||||
if(row[column]==0)
|
||||
ret.value = "ok";
|
||||
else {
|
||||
ret.value = "NOT OK";
|
||||
ret.color="red";
|
||||
}
|
||||
}
|
||||
else if(column == "latest-disco" && row[column] != null)
|
||||
ret.value = ((100*row[column]).toFixed(2))+" cm";
|
||||
else if(column == "e1bdvs" || column =="e5bdvs") {
|
||||
if(row[column] == 0)
|
||||
ret.value = "valid";
|
||||
|
|
@ -77,15 +90,21 @@ function maketable(str, arr)
|
|||
}
|
||||
|
||||
var sats={};
|
||||
|
||||
var lastseen=null;
|
||||
function update()
|
||||
{
|
||||
var seconds = 2;
|
||||
clearTimeout(repeat);
|
||||
repeat=setTimeout(update, 1000.0*seconds);
|
||||
|
||||
if(lastseen != null)
|
||||
d3.select("#freshness").html(lastseen.fromNow());
|
||||
|
||||
|
||||
d3.json("global", function(d) {
|
||||
d3.select('#facts').html("Galileo-UTC offset: <b>"+d["utc-offset-ns"].toFixed(2)+"</b> ns, Galileo-GPS offset: <b>"+d["gps-offset-ns"].toFixed(2)+"</b> ns<b>, "+d["leap-seconds"]+"</b> leap seconds");
|
||||
lastseen = moment(1000*d["last-seen"]);
|
||||
d3.select("#freshness").html(lastseen.fromNow());
|
||||
});
|
||||
|
||||
d3.json("svs", function(d) {
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ tr:nth-child(even) {background: #CCC}
|
|||
tr:nth-child(odd) {background: #FFF}
|
||||
</style>
|
||||
<body>
|
||||
Live:<br/>
|
||||
Last update: <span id="freshness"></span><br/>
|
||||
<table id="svs"></table>
|
||||
<hr>
|
||||
<p>
|
||||
|
|
@ -33,13 +33,15 @@ tr:nth-child(odd) {background: #FFF}
|
|||
|
||||
Some technical detail behind this setup can be found in <a href="https://ds9a.nl/articles/posts/galileo-notes/">this post</a>.
|
||||
|
||||
For updates, follow <a href="https://twitter.com/GalileoSats">@GalileoSats</a> on Twitter.
|
||||
For updates, follow <a href="https://twitter.com/GalileoSats">@GalileoSats</a> on Twitter, or join us on our IRC channel (chat) via the
|
||||
<a href="https://webchat.oftc.net/?channels=galileo">web gateway</a>.
|
||||
|
||||
The meaning of the fields is as follows:
|
||||
<table>
|
||||
<tr valign="top"><td>sv</td><td>Satellite Vehicle, an identifier for a Galileo satellite. Not the actual name of the satellite, other satellites could take over this number in case of failures</td></tr>
|
||||
<tr valign="top"><td>iod</td><td>Issue of Data. Satellites periodically get sent updates on their orbit & other details, each update has a new IOD number. It is coincidence that all SVs currently receive the same IOD numbers, this is by no means guaranteed. Currently however, if an SV has a lower IOD, it has not received new updates recently.</td></tr>
|
||||
<tr valign="top"><td>eph‑age‑m</td><td>Age of ephemeris in minutes. Denotes how old the current set of orbit data is. Could be very old if SV is out of sight (see below). An acceptable limit is 4 hours (240 minutes).</td></tr>
|
||||
<tr valign="top"><td>latest-disco</td><td>"jump" of the orbit prediction at the latest ephemeris change. Centimeters are good.</td></tr>
|
||||
<tr valign="top"><td>sisa</td><td>Signal In Space Accuracy, how well the position of an SV is known.</td></tr>
|
||||
<tr valign="top"><td>e1bhs, e1bdvs, e5bhs, e5bdvs</td><td>Health flags for E1 (common) and E5 (uncommon) frequencies.</td></tr>
|
||||
<tr valign="top"><td>a0, a1</td><td>Offset of the Galileo Standard Time to UTC. a0 is (more or less) the offset in nanoseconds, a1 is a measure of the rate of change</td></tr>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
#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 <time.h>
|
||||
#include "ubx.hh"
|
||||
#include "bits.hh"
|
||||
#include "minivec.hh"
|
||||
#include "navmon.pb.h"
|
||||
#include "ephemeris.hh"
|
||||
#include "gps.hh"
|
||||
#include <unistd.h>
|
||||
using namespace std;
|
||||
|
||||
static std::string humanTime(time_t t)
|
||||
{
|
||||
struct tm tm;
|
||||
gmtime_r(&t, &tm);
|
||||
|
||||
char buffer[80];
|
||||
strftime(buffer, sizeof(buffer), "%a, %d %b %Y %T %z", &tm);
|
||||
return buffer;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
for(;;) {
|
||||
char bert[4];
|
||||
if(read(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(read(0, &len, 2) != 2)
|
||||
break;
|
||||
len = htons(len);
|
||||
char buffer[len];
|
||||
if(read(0, buffer, len) != len)
|
||||
break;
|
||||
|
||||
NavMonMessage nmm;
|
||||
nmm.ParseFromString(string(buffer, len));
|
||||
cout<<humanTime(nmm.localutcseconds())<<" "<<nmm.localutcnanoseconds()<<" ";
|
||||
cout<<"src "<<nmm.sourceid()<< " ";
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||
cout << "galileo inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" wtype "<< wtype << endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GPSInavType) {
|
||||
int sv = nmm.gpsi().gnsssv();
|
||||
cout<<"GPS "<<sv<<endl;
|
||||
}
|
||||
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
cout<<"ECEF"<<endl;
|
||||
// XXX!! this has to deal with source id!
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<endl;
|
||||
}
|
||||
else {
|
||||
cout<<"Unknown type "<< (int)nmm.type()<<endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
28
navnexus.cc
28
navnexus.cc
|
|
@ -22,9 +22,6 @@ set<int> g_clients;
|
|||
|
||||
std::string g_storage;
|
||||
|
||||
std::multimap<pair<uint32_t,uint32_t>, string> g_history;
|
||||
|
||||
|
||||
void unixDie(const std::string& str)
|
||||
{
|
||||
throw std::runtime_error(str+string(": ")+string(strerror(errno)));
|
||||
|
|
@ -64,19 +61,15 @@ try
|
|||
cerr<<"New downstream client "<<client.toStringWithPort() << endl;
|
||||
|
||||
pair<uint64_t, uint64_t> start = {0,0};
|
||||
start.first = time(0) - 1800;
|
||||
|
||||
start.first = time(0) - 4*3600; // 4 hours of backlog
|
||||
|
||||
// so we have a ton of files, and internally these are not ordered
|
||||
map<string,uint32_t> fpos;
|
||||
for(;;) {
|
||||
auto srcs = getSources();
|
||||
vector<NavMonMessage> nmms;
|
||||
for(const auto& s: srcs) {
|
||||
time_t t = time(0);
|
||||
|
||||
cout<<s <<" -> "<<getPath(g_storage, t, s) << " & " << getPath(g_storage, t-3600, s) << endl;
|
||||
|
||||
string fname = getPath(g_storage, t, s);
|
||||
for(const auto& src: srcs) {
|
||||
string fname = getPath(g_storage, start.first, src);
|
||||
int fd = open(fname.c_str(), O_RDONLY);
|
||||
if(fd < 0)
|
||||
continue;
|
||||
|
|
@ -91,7 +84,8 @@ try
|
|||
|
||||
uint32_t looked=0;
|
||||
while(getNMM(fd, nmm, offset)) {
|
||||
if(make_pair(nmm.localutcseconds(), nmm.localutcnanoseconds()) > start) {
|
||||
// don't drop data that is only 5 seconds too old
|
||||
if(make_pair(nmm.localutcseconds() + 5, nmm.localutcnanoseconds()) >= start) {
|
||||
nmms.push_back(nmm);
|
||||
}
|
||||
++looked;
|
||||
|
|
@ -115,9 +109,13 @@ try
|
|||
buf+=out;
|
||||
SWriten(clientfd, buf);
|
||||
}
|
||||
if(!nmms.empty())
|
||||
start = {nmms.rbegin()->localutcseconds(), nmms.rbegin()->localutcnanoseconds()};
|
||||
sleep(1);
|
||||
if(3600 + start.first - (start.first%3600) < time(0))
|
||||
start.first = 3600 + start.first - (start.first%3600);
|
||||
else {
|
||||
if(!nmms.empty())
|
||||
start = {nmms.rbegin()->localutcseconds(), nmms.rbegin()->localutcnanoseconds()};
|
||||
sleep(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
|
|
|
|||
257
navparse.cc
257
navparse.cc
|
|
@ -19,7 +19,7 @@
|
|||
#include "navmon.pb.h"
|
||||
#include "ephemeris.hh"
|
||||
#include "gps.hh"
|
||||
|
||||
#include <optional>
|
||||
using namespace std;
|
||||
|
||||
struct EofException{};
|
||||
|
|
@ -70,8 +70,12 @@ struct SVIOD
|
|||
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
|
||||
|
||||
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
|
||||
uint16_t t0c; // clock epoch
|
||||
int32_t af0, af1;
|
||||
// 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 , af1;
|
||||
// 2^-59
|
||||
int8_t af2;
|
||||
|
||||
uint8_t sisa;
|
||||
|
|
@ -93,7 +97,7 @@ void SVIOD::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|||
words[wtype]=true;
|
||||
gnssid = 2;
|
||||
if(wtype == 1) {
|
||||
t0e = getbitu(&page[0], 16, 14);
|
||||
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);
|
||||
|
|
@ -136,6 +140,10 @@ struct SVPerRecv
|
|||
};
|
||||
|
||||
|
||||
/* 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};
|
||||
|
|
@ -146,16 +154,19 @@ struct SVStat
|
|||
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};
|
||||
uint16_t wn{0}; // we put the "unrolled" week number here!
|
||||
uint32_t tow{0}; // "last seen"
|
||||
int32_t a0{0}, a1{0}, t0t{0}, wn0t{0};
|
||||
//
|
||||
// 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
|
||||
int ura, af0, af1, af2, t0c, iod; // GPS parameters that should not be here XXX
|
||||
// 1 2^-31 2^-43 2^-55 16 second
|
||||
int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX
|
||||
map<uint64_t, SVPerRecv> perrecv;
|
||||
|
||||
pair<uint32_t, double> deltaHz;
|
||||
double latestDisco{-1};
|
||||
|
||||
map<int, SVIOD> iods;
|
||||
|
|
@ -171,7 +182,7 @@ struct SVStat
|
|||
{
|
||||
prevIOD.first = -1;
|
||||
}
|
||||
void checkCompleteAndClean();
|
||||
void checkCompleteAndClean(int iod);
|
||||
};
|
||||
|
||||
bool SVStat::completeIOD() const
|
||||
|
|
@ -197,7 +208,7 @@ SVIOD SVStat::liveIOD() const
|
|||
throw std::runtime_error("Asked for unknown IOD");
|
||||
}
|
||||
|
||||
void SVStat::checkCompleteAndClean()
|
||||
void SVStat::checkCompleteAndClean(int iod)
|
||||
{
|
||||
if(iods[iod].complete()) {
|
||||
for(const auto& i : iods) {
|
||||
|
|
@ -225,7 +236,7 @@ void SVStat::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|||
else if(wtype >=1 && wtype <= 4) { // ephemeris
|
||||
uint16_t iod = getbitu(&page[0], 6, 10);
|
||||
iods[iod].addGalileoWord(page);
|
||||
checkCompleteAndClean();
|
||||
checkCompleteAndClean(iod);
|
||||
}
|
||||
else if(wtype==5) { // disturbance, health, time
|
||||
ai0 = getbitu(&page[0], 6, 11);
|
||||
|
|
@ -255,8 +266,7 @@ void SVStat::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|||
a0 = getbits(&page[0], 6, 32);
|
||||
a1 = getbits(&page[0], 38, 24);
|
||||
dtLS = getbits(&page[0], 62, 8);
|
||||
cerr<<"Setting a0,a1 to "<<a0<<", "<<a1<<endl;
|
||||
t0t = getbitu(&page[0], 70, 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);
|
||||
|
|
@ -309,21 +319,29 @@ int latestTow(int gnssid)
|
|||
|
||||
|
||||
|
||||
uint64_t nanoTime(int wn, int tow)
|
||||
uint64_t nanoTime(int gnssid, int wn, int tow)
|
||||
{
|
||||
return 1000000000ULL*(935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!!
|
||||
int offset = (gnssid == 0) ? 315964800 : 935280000;
|
||||
|
||||
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); // Leap!!
|
||||
}
|
||||
|
||||
uint64_t utcFromGST(int wn, int tow)
|
||||
{
|
||||
return (935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!!
|
||||
return (935280000 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGST(int wn, double tow)
|
||||
{
|
||||
return (935280000.0 + wn * 7*86400 + tow - g_dtLS); // Leap!!
|
||||
return (935280000.0 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGPS(int wn, double tow)
|
||||
{
|
||||
return (315964800 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
|
||||
|
||||
struct InfluxPusher
|
||||
{
|
||||
|
|
@ -332,15 +350,18 @@ struct InfluxPusher
|
|||
}
|
||||
void addValue( const pair<pair<int,int>,SVStat>& ent, string_view name, auto value)
|
||||
{
|
||||
d_buffer+= string(name) +",sv=" +to_string(ent.first.second)+",gnssid="+to_string(ent.first.first)+" value="+to_string(value)+
|
||||
" "+to_string(nanoTime(ent.second.wn, ent.second.tow))+"\n";
|
||||
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();
|
||||
}
|
||||
|
||||
void addValue(pair<int,int> id, string_view name, auto value)
|
||||
{
|
||||
if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0)
|
||||
return;
|
||||
d_buffer+= string(name) +",gnssid="+to_string(id.first)+",sv=" +to_string(id.second) + " value="+to_string(value)+" "+
|
||||
to_string(nanoTime(g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
||||
to_string(nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
||||
|
||||
checkSend();
|
||||
}
|
||||
|
||||
|
|
@ -350,6 +371,7 @@ struct InfluxPusher
|
|||
string buffer;
|
||||
buffer.swap(d_buffer);
|
||||
// thread t([buffer,this]() {
|
||||
if(d_dbname != "null")
|
||||
doSend(buffer);
|
||||
// });
|
||||
// t.detach();
|
||||
|
|
@ -421,6 +443,38 @@ int ephAge(int tow, int t0e)
|
|||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
try
|
||||
{
|
||||
|
|
@ -447,7 +501,7 @@ try
|
|||
if(s.first.first != 2) // Galileo only
|
||||
continue;
|
||||
int dw = (uint8_t)s.second.wn - s.second.wn0t;
|
||||
int age = dw * 7 * 86400 + s.second.tow - s.second.t0t * 3600;
|
||||
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;
|
||||
|
|
@ -482,6 +536,11 @@ try
|
|||
|
||||
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
|
||||
|
|
@ -492,7 +551,7 @@ try
|
|||
item["sisa"]=humanUra(s.second.ura);
|
||||
else
|
||||
item["sisa"]=humanSisa(s.second.liveIOD().sisa);
|
||||
item["eph-age-m"] = ephAge(s.second.tow, 60*s.second.liveIOD().t0e)/60.0;
|
||||
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;
|
||||
|
|
@ -518,18 +577,28 @@ try
|
|||
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;
|
||||
item["a0g"]=s.second.a0g;
|
||||
item["a1g"]=s.second.a1g;
|
||||
item["e5bdvs"]=s.second.e5bdvs;
|
||||
item["e1bdvs"]=s.second.e1bdvs;
|
||||
item["e5bhs"]=s.second.e5bhs;
|
||||
item["e1bhs"]=s.second.e1bhs;
|
||||
item["gpshealth"]=s.second.gpshealth;
|
||||
if(s.first.first == 2) {
|
||||
item["a0g"]=s.second.a0g;
|
||||
item["a1g"]=s.second.a1g;
|
||||
item["e5bdvs"]=s.second.e5bdvs;
|
||||
item["e1bdvs"]=s.second.e1bdvs;
|
||||
item["e5bhs"]=s.second.e5bhs;
|
||||
item["e1bhs"]=s.second.e1bhs;
|
||||
}
|
||||
else
|
||||
item["gpshealth"]=s.second.gpshealth;
|
||||
|
||||
nlohmann::json perrecv = nlohmann::json::object();
|
||||
for(const auto& pr : s.second.perrecv) {
|
||||
|
|
@ -593,7 +662,7 @@ try
|
|||
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();
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
|
||||
|
||||
// THIS HAS TO SPLIT OUT PER SOURCE
|
||||
|
|
@ -607,6 +676,7 @@ try
|
|||
int sv = nmm.gi().gnsssv();
|
||||
pair<int, int> id={2,sv};
|
||||
g_svstats[id].wn = nmm.gi().gnsswn();
|
||||
|
||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||
if(1) {
|
||||
// cout<<sv <<"\t" << wtype << "\t" << nmm.gi().gnsstow() << "\t"<< nmm.sourceid() << endl;
|
||||
|
|
@ -639,7 +709,7 @@ try
|
|||
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);
|
||||
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);
|
||||
|
||||
|
|
@ -672,7 +742,7 @@ try
|
|||
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 * 3600;
|
||||
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));
|
||||
|
|
@ -704,7 +774,7 @@ try
|
|||
// 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 * 60);
|
||||
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<<" ("<<
|
||||
|
|
@ -719,36 +789,39 @@ try
|
|||
// 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;
|
||||
|
||||
double hours = ((ent.second.liveIOD().t0e - ent.second.prevIOD.second.t0e)/60.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");
|
||||
|
||||
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);
|
||||
|
||||
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 * 60 + 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";
|
||||
|
||||
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();
|
||||
|
|
@ -764,12 +837,7 @@ try
|
|||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
int sv = nmm.rfd().gnsssv();
|
||||
pair<int,int> id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()};
|
||||
if(!nmm.rfd().gnssid()) {// GPS
|
||||
dopplercsv << std::fixed << utcFromGST(nmm.rfd().rcvwn(), nmm.rfd().rcvtow()) <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << 0 << " " << 0 << " " << 0 <<" " << nmm.rfd().locktimems()<<" " << 0 << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
||||
nmm.rfd().dostd() << endl;
|
||||
|
||||
}
|
||||
else if(nmm.rfd().gnssid() ==2 && g_svstats[id].completeIOD()) {
|
||||
if(g_svstats[id].completeIOD()) {
|
||||
Point sat;
|
||||
Point us=g_ourpos;
|
||||
|
||||
|
|
@ -791,22 +859,63 @@ try
|
|||
us2sat.norm();
|
||||
double radvel=us2sat.inner(speed);
|
||||
double c=299792458;
|
||||
double galileol1f = 1575.42 * 1000000; // frequency
|
||||
double preddop = -galileol1f*radvel/c;
|
||||
double freq = 1575.42 * 1000000; // frequency
|
||||
double preddop = -freq*radvel/c;
|
||||
|
||||
// be careful with time here -
|
||||
double ephage = ephAge(nmm.rfd().rcvtow(), g_svstats[id].liveIOD().t0e*60);
|
||||
double ephage = ephAge(nmm.rfd().rcvtow(), g_svstats[id].liveIOD().t0e);
|
||||
// cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "<<nmm.rfd().doppler()<<endl;
|
||||
dopplercsv << std::fixed << utcFromGST(nmm.rfd().rcvwn(), nmm.rfd().rcvtow()) <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " << nmm.rfd().locktimems()<<" " <<ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
||||
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()<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " << nmm.rfd().locktimems()<<" " <<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() - preddop};
|
||||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - preddop);
|
||||
auto corr = getHzCorrection(t);
|
||||
if(corr) {
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - 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()};
|
||||
auto& svstat = g_svstats[id];
|
||||
parseGPSMessage(cond, svstat);
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
auto& svstat = g_svstats[id];
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
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 {
|
||||
cout<<"Unknown type "<< (int)nmm.type()<<endl;
|
||||
|
|
|
|||
|
|
@ -541,7 +541,7 @@ int main(int argc, char** argv)
|
|||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||
nmm.set_sourceid(g_srcid);
|
||||
// cerr<<"GPS frame, numwords: "<<(int)payload[4]<<", version: "<<(int)payload[6]<<endl;
|
||||
unsigned int wn{18}, tow;
|
||||
unsigned int wn{2048+18}, tow;
|
||||
auto gpsframe = getGPSFromSFRBXMsg(id.second, payload);
|
||||
auto cond = getCondensedGPSMessage(gpsframe);
|
||||
tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
|
||||
|
|
|
|||
Loading…
Reference in New Issue