spacecruft
/
galmon
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

move to protobuf

pull/1/head
bert hubert 2019-08-09 15:58:52 +02:00
parent 67e90b6fa2
commit 28479b4faf
10 changed files with 1618 additions and 358 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Makefile
View File

@ -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 = ubxparse ubxdisplay minread ubxtool
PROGRAMS = ubxparse navparse ubxdisplay minread ubxtool
all: $(PROGRAMS)
@ -14,9 +14,16 @@ SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simpl
ubxparse: ubxparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o
g++ -std=gnu++17 $^ -o $@ -pthread -lncurses -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl # -lwslay
navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o
g++ -std=gnu++17 $^ -o $@ -pthread -lncurses -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf # -lwslay
navmon.pb.h: navmon.proto
protoc --cpp_out=./ navmon.proto
ubxdisplay: ubxdisplay.o ext/fmt-5.2.1/src/format.o
g++ -std=gnu++17 $^ -o $@ -pthread -lncurses
ubxtool: ubxtool.o ubx.o ext/fmt-5.2.1/src/format.o
g++ -std=gnu++17 $^ -o $@
ubxtool: ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o navmon.pb.o
g++ -std=gnu++17 $^ -o $@ -lprotobuf

7
README.md
View File

@ -32,3 +32,10 @@ The magic value is there to help us resync from partially written data.
The whole goal is that we can continue to rebuild the database by
rerunning 'navstore' and 'navinflux'.
ubxtool
-------
* Will also spool raw serial data to disk (in a filename that includes the
start date)
* Can also read from disk
* Careful to add the right timestamps

26
galileo.cc 100644
View File

@ -0,0 +1,26 @@
#include "bits.hh"
#include "galileo.hh"
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn)
{
unsigned int wtype = getbitu(&inav[0], 0, 6);
if(wtype==0) {
if(getbitu(&inav[0], 6,2) == 2) {
*wn = getbitu(&inav[0], 96, 12);
*satTOW = getbitu(&inav[0], 108, 20);
return true;
}
}
else if(wtype==5) {
*wn = getbitu(&inav[0], 73, 12);
*satTOW = getbitu(&inav[0], 85, 20);
return true;
}
else if(wtype==6) {
// !! NO WN!!
*satTOW=getbitu(&inav[0], 105, 20);
return true;
}
return false;
}

5
galileo.hh 100644
View File

@ -0,0 +1,5 @@
#pragma once
#include <stdint.h>
#include <string>
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn);

53
navmon.proto 100644
View File

@ -0,0 +1,53 @@
syntax = "proto2";
message NavMonMessage {
enum Type {
ReceptionDataType = 1;
ObserverPositionType = 2;
GalileoInavType = 3;
RFDataType = 4;
}
required uint64 sourceID = 1;
required Type type = 2;
required uint64 localUtcSeconds = 3;
required uint64 localUtcNanoseconds = 4;
message GalileoInav {
required uint32 gnssWN =1;
required uint32 gnssTOW =2; // INTEGERS!
required uint32 gnssID =3;
required uint32 gnssSV =4;
required bytes contents =5;
}
message ReceptionData {
required uint32 gnssID =1;
required uint32 gnssSV =2;
required uint32 db =3;
required uint32 el =4;
required uint32 azi =5;
required double prRes =6;
}
message RFData {
required uint32 gnssID =1;
required uint32 gnssSV =2;
required double doppler =3;
required double carrierphase =4;
}
message ObserverPosition {
required double x = 1;
required double y = 2;
required double z = 3;
required double accCm = 4;
}
optional GalileoInav gi=5;
optional ReceptionData rd=6;
optional RFData rfd=7;
optional ObserverPosition op=8;
}

902
navparse.cc 100644
View File

@ -0,0 +1,902 @@
#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 <curses.h>
#include <vector>
#include <boost/algorithm/string.hpp>
#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"
using namespace std;
struct EofException{};
Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000);
int g_dtLS{18};
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";
}
struct SVIOD
{
std::bitset<32> words;
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};
uint16_t t0c; // clock epoch
int32_t af0, af1;
int8_t af2;
uint8_t sisa;
uint32_t wn{0}, tow{0};
bool complete() const
{
return words[1] && words[2] && words[3] && words[4];
}
void addWord(std::basic_string_view<uint8_t> page);
};
void SVIOD::addWord(std::basic_string_view<uint8_t> page)
{
uint8_t wtype = getbitu(&page[0], 0, 6);
words[wtype]=true;
if(wtype == 1) {
t0e = getbitu(&page[0], 16, 14);
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 SVStat
{
uint8_t e5bhs{0}, e1bhs{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};
uint32_t tow{0}; // "last seen"
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
int16_t el{-1}, azi{-1}, db{-1};
map<int, SVIOD> iods;
void addWord(std::basic_string_view<uint8_t> page);
bool completeIOD() const;
uint16_t getIOD() const;
SVIOD liveIOD() const;
pair<int,SVIOD> prevIOD{-1, SVIOD()};
void clearPrev()
{
prevIOD.first = -1;
}
};
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 unknown IOD");
}
SVIOD SVStat::liveIOD() const
{
if(auto iter = iods.find(getIOD()); iter != iods.end())
return iter->second;
throw std::runtime_error("Asked for unknown IOD");
}
void SVStat::addWord(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);
tow = getbitu(&page[0], 108, 20);
}
}
else if(wtype >=1 && wtype <= 4) { // ephemeris
uint16_t iod = getbitu(&page[0], 6, 10);
iods[iod].addWord(page);
if(iods[iod].complete()) {
for(const auto& i : iods) {
if(i.first != iod && i.second.complete())
prevIOD=i;
}
SVIOD latest = iods[iod];
iods.clear();
iods[iod] = latest;
}
}
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);
tow = getbitu(&page[0], 85, 20);
}
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);
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;
tow = getbitu(&page[0], 105, 20);
}
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);
}
}
double todeg(double rad)
{
return 360 * rad/(2*M_PI);
}
void getCoordinates(int wn, double tow, const SVIOD& iod, Point* p, bool quiet=true)
{
// here goes
constexpr double mu = 3.986004418 * pow(10.0, 14.0);
constexpr double omegaE = 7.2921151467 * pow(10.0, -5);
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 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);
double cuc = 1.0*iod.cuc / (1LL<<29);
double cus = 1.0*iod.cus / (1LL<<29);
double crc = 1.0*iod.crc / (1LL<<5);
double crs = 1.0*iod.crs / (1LL<<5);
double cic = 1.0*iod.cic / (1LL<<29);
double cis = 1.0*iod.cis / (1LL<<29);
double idot = M_PI * 1.0*iod.idot / (1LL<<43);
double i0 = M_PI * 1.0*iod.i0 / (1LL << 31);
double Omegadot = M_PI * 1.0*iod.omegadot / (1LL << 43);
double Omega0 = M_PI * 1.0*iod.omega0 / (1LL << 31);
// NO IOD BEYOND THIS POINT!
if(!quiet) {
cout << "sqrtA = "<< sqrtA << endl;
cout << "deltan = "<< deltan << endl;
cout << "t0e = "<< t0e << endl;
cout << "m0 = "<< m0 << " ("<<todeg(m0)<<")"<<endl;
cout << "e = "<< e << endl;
cout << "omega = " << omega << " ("<<todeg(omega)<<")"<<endl;
cout << "idot = " << idot <<endl;
cout << "i0 = " << i0 << " ("<<todeg(i0)<<")"<<endl;
cout << "cuc = " << cuc << endl;
cout << "cus = " << cus << endl;
cout << "crc = " << crc << endl;
cout << "crs = " << crs << endl;
cout << "cic = " << cic << endl;
cout << "cis = " << cis << endl;
cout << "Omega0 = " << Omega0 << " ("<<todeg(Omega0)<<")"<<endl;
cout << "Omegadot = " << Omegadot << " ("<<todeg(Omegadot)<< ")"<<endl;
}
double A = pow(sqrtA, 2.0);
double A3 = pow(sqrtA, 6.0);
double n0 = sqrt(mu/A3);
double tk = tow - t0e; // in seconds, ignores WN!
double n = n0 + deltan;
if(!quiet)
cout <<"tk: "<<tk<<", n0: "<<n0<<", deltan: "<<deltan<<", n: "<<n<<endl;
double M = m0 + n * tk;
if(!quiet)
cout << " M = m0 + n * tk = "<<m0 << " + " << n << " * " << tk <<endl;
double E = M;
for(int k =0 ; k < 10; ++k) {
if(!quiet)
cout<<"M = "<<M<<", E = "<< E << endl;
E = M + e * sin(E);
}
// M = E - e * sin(E) -> E = M + e * sin(E)
double nu2 = M + e*2*sin(M) +
e *e * (5.0/4.0) * sin(2*M) -
e*e*e * (0.25*sin(M) - (13.0/12.0)*sin(3*M));
double corr = e*e*e*e * (103*sin(4*M) - 44*sin(2*M)) / 96.0 +
e*e*e*e*e * (1097*sin(5*M) - 645*sin(3*M) + 50 *sin(M))/960.0 +
e*e*e*e*e*e * (1223*sin(6*M) - 902*sin(4*M) + 85 *sin(2*M))/960.0;
if(!quiet) {
double nu1 = atan( ((sqrt(1-e*e) * sin(E)) / (1 - e * cos(E)) ) /
((cos(E) - e)/ (1-e*cos(E)))
);
double nu2 = atan( (sqrt(1-e*e) * sin(E)) /
(cos(E) - e)
);
double nu3 = 2* atan( sqrt((1+e)/(1-e)) * tan(E/2));
cout << "e: "<<e<<", M: "<< M<<endl;
cout <<" nu sis: "<<nu1<< " / +pi = " << nu1 +M_PI << endl;
cout <<" nu ?: "<<nu2<< " / +pi = " << nu2 +M_PI << endl;
cout <<" nu fourier/esa: "<<nu2<< " + " << corr <<" = " << nu2 + corr<<endl;
cout <<" nu wikipedia: "<<nu3<< " / +pi = " <<nu3 +M_PI << endl;
}
double nu = nu2 + corr;
// https://en.wikipedia.org/wiki/True_anomaly is good
double psi = nu + omega;
if(!quiet) {
cout<<"psi = nu + omega = " << nu <<" + "<<omega<< " = " << psi << "\n";
}
double deltau = cus * sin(2*psi) + cuc * cos(2*psi);
double deltar = crs * sin(2*psi) + crc * cos(2*psi);
double deltai = cis * sin(2*psi) + cic * cos(2*psi);
double u = psi + deltau;
double r = A * (1- e * cos(E)) + deltar;
double xprime = r*cos(u), yprime = r*sin(u);
if(!quiet) {
cout<<"u = psi + deltau = "<< psi <<" + " << deltau << " = "<<u<<"\n";
cout << "calculated r = "<< r << " (" << (r/1000.0) <<"km)"<<endl;
cout << "xprime: "<<xprime<<", yprime: "<<yprime<<endl;
}
double Omega = Omega0 + (Omegadot - omegaE)*tk - omegaE * t0e;
double i = i0 + deltai + idot * tk;
p->x = xprime * cos(Omega) - yprime * cos(i) * sin(Omega);
p->y = xprime * sin(Omega) + yprime * cos(i) * cos(Omega);
p->z = yprime * sin(i);
if(!quiet) {
Point core(0.0, .0, .0);
Vector radius(core, *p);
cout << radius.length() << " calculated r "<<endl;
}
}
void getSpeed(int wn, double tow, const SVIOD& iod, Vector* v)
{
Point a, b;
getCoordinates(wn, tow-0.5, iod, &a);
getCoordinates(wn, tow+0.5, iod, &b);
*v = Vector(a, b);
}
std::map<int, SVStat> g_svstats;
int latestWN()
{
map<int, int> ages;
for(const auto& s: g_svstats)
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
if(ages.empty())
throw runtime_error("Asked for latest WN: we don't know it yet");
return g_svstats[ages.rbegin()->second].wn;
}
int latestTow()
{
map<int, int> ages;
for(const auto& s: g_svstats)
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
if(ages.empty())
throw runtime_error("Asked for latest WN: we don't know it yet");
return g_svstats[ages.rbegin()->second].tow;
}
uint64_t nanoTime(int wn, int tow)
{
return 1000000000ULL*(935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!!
}
uint64_t utcFromGST(int wn, int tow)
{
return (935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!!
}
double utcFromGST(int wn, double tow)
{
return (935280000.0 + wn * 7*86400 + tow - g_dtLS); // Leap!!
}
struct InfluxPusher
{
explicit InfluxPusher(std::string_view dbname) : d_dbname(dbname)
{
}
void addValue( const pair<int,SVStat>& ent, string_view name, auto value)
{
d_buffer+= string(name) +",sv=" +to_string(ent.first)+" value="+to_string(value)+
" "+to_string(nanoTime(ent.second.wn, ent.second.tow))+"\n";
checkSend();
}
void addValue(int sv, string_view name, auto value)
{
d_buffer+= string(name) +",sv=" +to_string(sv) + " value="+to_string(value)+" "+
to_string(nanoTime(g_svstats[sv].wn, g_svstats[sv].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]() {
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()
{
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;
}
/* | t0e tow | - > tow - t0e, <3.5 days, so ok
| t0e tow | -> tow - t0e > 3.5 days, so
7*86400 - tow + t0e
| tow t0e | -> 7*86400 - tow + t0e > 3.5 days, so
tow - t0e (negative age)
| tow t0e | -> 7*86400 - tow + t0e < 3.5 days, ok
*/
int ephAge(int tow, int t0e)
{
unsigned int diff;
unsigned int halfweek = 0.5*7*86400;
if(t0e < tow) {
diff = tow - t0e;
if(diff < halfweek)
return diff;
else
return (7*86400 - tow) + t0e;
}
else { // "t0e in future"
diff = 7*86400 - t0e + tow;
if(diff < halfweek)
return diff;
else
return tow - t0e; // in the future, negative age
}
}
int main(int argc, char** argv)
try
{
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;
map<int, int> utcstats, gpsgststats;
for(const auto& s: g_svstats) {
int dw = (uint8_t)s.second.wn - s.second.wn0t;
int age = dw * 7 * 86400 + s.second.tow - s.second.t0t * 3600;
utcstats[age]=s.first;
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;
}
if(utcstats.empty()) {
ret["utc-offset-ns"]=nullptr;
}
else {
int sv = utcstats.begin()->second; // freshest SV
long shift = g_svstats[sv].a0 * (1LL<<20) + g_svstats[sv].a1 * utcstats.begin()->first; // in 2^-50 seconds units
ret["utc-offset-ns"] = 1.073741824*ldexp(shift, -20);
ret["leap-second-planned"] = (g_svstats[sv].dtLSF != g_svstats[sv].dtLS);
}
if(gpsgststats.empty()) {
ret["gps-offset-ns"]=nullptr;
}
else {
int sv = gpsgststats.begin()->second; // freshest SV
long shift = g_svstats[sv].a0g * (1U<<16) + g_svstats[sv].a1g * utcstats.begin()->first; // in 2^-51 seconds units
ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21);
}
return ret;
});
h2s.addHandler("/svs", [](auto handler, auto req) {
nlohmann::json ret = nlohmann::json::object();
for(const auto& s: g_svstats)
if(s.second.completeIOD()) {
nlohmann::json item = nlohmann::json::object();
item["iod"]=s.second.getIOD();
item["sisa"]=humanSisa(s.second.liveIOD().sisa);
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["elev"]=s.second.el;
item["db"]=s.second.db;
item["eph-age-m"] = ephAge(s.second.tow, 60*s.second.liveIOD().t0e)/60.0;
item["last-seen-s"] = s.second.tow ? (7*86400*(s.second.wn - s.second.wn) + latestTow() - (int)s.second.tow) : -1;
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(latestWN(), latestTow(), s.second.liveIOD(), &p);
Vector core2us(core, our);
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
/* https://gis.stackexchange.com/questions/58923/calculating-view-angle
to calculate elevation:
Cos(elevation) = (x*dx + y*dy + z*dz) / Sqrt((x^2+y^2+z^2)*(dx^2+dy^2+dz^2))
Obtain its principal inverse cosine. Subtract this from 90 degrees if you want
the angle of view relative to a nominal horizon. This is the "elevation."
NOTE! x = you on the ground!
*/
double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
double deg = 180.0* (elev/M_PI);
item["calc-elev"] = 90 - deg;
cout<<s.first<<" calc elev radians "<< elev << ", deg "<< deg <<" calc-elev "<<(90-deg)<<", from ublox "<< s.second.el<<endl <<std::fixed<<" (" << our.x << ", "<< our.y <<", "<<our.z<<") -> ("
<< p.x << ", "<< p.y <<", "<< p.z<<") "<<endl;// << sqrt(ourx*ourx + oury*oury + ourz*ourz) <<" - " << sqrt(x*x+y*y+z*z) <<endl;
item["x"]=p.x;
item["y"]=p.y;
item["z"]=p.z;
item["wn"] = s.second.wn;
item["tow"] = s.second.tow;
ret[std::to_string(s.first)] = 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 csv("iod.csv");
ofstream csv2("toe.csv");
csv<<"timestamp sv iod sisa"<<endl;
csv2<<"timestamp sv tow toe"<<endl;
ofstream gstutc("gstutc.csv");
gstutc << "timestamp sv tow t0t age rawshift nsecshift a0 a1" <<endl;
ofstream gstgps("gstgps.csv");
gstgps << "timestamp sv tow t0g age rawshift nsecshift a0g a1g" <<endl;
ofstream sisacsv("sisa.csv");
sisacsv << "timestamp sv sisa"<<endl;
ofstream clockcsv("clock.csv");
clockcsv <<"timestamp sv af0 af1 af2 t0c age offset"<<endl;
ofstream dopplercsv("doppler.csv");
dopplercsv<<"timestamp gnssid sv prmes cpmes doppler preddop distance radvel locktimems iod_age prstd cpstd dostd trkstat"<<endl;
try {
for(;;) {
char bert[4];
if(read(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t')
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));
if(nmm.type() == NavMonMessage::ReceptionDataType) {
int sv = nmm.rd().gnsssv();
g_svstats[sv].db = nmm.rd().db();
g_svstats[sv].el = nmm.rd().el();
g_svstats[sv].azi = nmm.rd().azi();
idb.addValue(sv, "db", g_svstats[sv].db);
idb.addValue(sv, "elev", g_svstats[sv].el);
idb.addValue(sv, "azi", g_svstats[sv].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();
g_svstats[sv].wn = nmm.gi().gnsswn();
g_svstats[sv].tow = nmm.gi().gnsstow();
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
// for(auto& c : inav)
// fmt::printf("%02x ", c);
unsigned int wtype = getbitu(&inav[0], 0, 6);
g_svstats[sv].addWord(inav);
if(g_svstats[sv].e1bhs || g_svstats[sv].e5bhs || g_svstats[sv].e1bdvs || g_svstats[sv].e5bdvs) {
if(sv != 18 && sv != 14)
cout << "sv "<<sv<<" health: " << g_svstats[sv].e1bhs <<" " << g_svstats[sv].e5bhs <<" " << g_svstats[sv].e1bdvs <<" "<< g_svstats[sv].e5bdvs <<endl;
}
if(wtype >=1 && wtype <= 4) { // ephemeris
uint16_t iod = getbitu(&inav[0], 6, 10);
if(wtype == 3) {
idb.addValue(sv, "sisa", g_svstats[sv].iods[iod].sisa);
}
else if(wtype == 4) {
idb.addValue(sv, "af0", g_svstats[sv].iods[iod].af0);
idb.addValue(sv, "af1", g_svstats[sv].iods[iod].af1);
idb.addValue(sv, "af2", g_svstats[sv].iods[iod].af2);
idb.addValue(sv, "t0c", g_svstats[sv].iods[iod].t0c);
double age = ephAge(g_svstats[sv].tow, g_svstats[sv].iods[iod].t0c * 60);
double offset = ldexp(1000.0*(1.0*g_svstats[sv].iods[iod].af0 + ldexp(age*g_svstats[sv].iods[iod].af1, -12)), -34);
idb.addValue(sv, "atomic_offset_ns", 1000000.0*offset);
}
else
;
}
else if(wtype == 5) {
idb.addValue(sv, "ai0", g_svstats[sv].ai0);
idb.addValue(sv, "ai1", g_svstats[sv].ai1);
idb.addValue(sv, "ai2", g_svstats[sv].ai2);
idb.addValue(sv, "sf1", g_svstats[sv].sf1);
idb.addValue(sv, "sf2", g_svstats[sv].sf2);
idb.addValue(sv, "sf3", g_svstats[sv].sf3);
idb.addValue(sv, "sf4", g_svstats[sv].sf4);
idb.addValue(sv, "sf5", g_svstats[sv].sf5);
idb.addValue(sv, "BGDE1E5a", g_svstats[sv].BGDE1E5a);
idb.addValue(sv, "BGDE1E5b", g_svstats[sv].BGDE1E5b);
idb.addValue(sv, "e1bhs", g_svstats[sv].e1bhs);
idb.addValue(sv, "e5bhs", g_svstats[sv].e5bhs);
idb.addValue(sv, "e5bdvs", g_svstats[sv].e5bdvs);
idb.addValue(sv, "e1bdvs", g_svstats[sv].e1bdvs);
}
else if(wtype == 6) { // GST-UTC
idb.addValue(sv, "a0", g_svstats[sv].a0);
idb.addValue(sv, "a1", g_svstats[sv].a1);
g_dtLS = g_svstats[sv].dtLS;
}
else if(wtype == 10) { // GSTT GPS
idb.addValue(sv, "a0g", g_svstats[sv].a0g);
idb.addValue(sv, "a1g", g_svstats[sv].a1g);
idb.addValue(sv, "t0g", g_svstats[sv].t0g);
}
for(auto& ent : g_svstats) {
// fmt::printf("%2d\t", ent.first);
if(ent.second.completeIOD() && ent.second.prevIOD.first >= 0) {
time_t t = utcFromGST((int)ent.second.wn, (int)ent.second.tow);
sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl;
// 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);
clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + ent.second.wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset<<endl;
int ephage = ephAge(ent.second.tow, ent.second.prevIOD.second.t0e * 60);
if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) {
cout<<humanTime(ent.second.wn, ent.second.tow)<<" sv "<<ent.first<<" 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;
double hours = ((ent.second.liveIOD().t0e - ent.second.prevIOD.second.t0e)/60.0);
double disco = Vector(p, oldp).length();
cout<<ent.first<<" discontinuity after "<< hours<<" hours: "<< disco <<endl;
idb.addValue(sv, "iod-actual", ent.second.getIOD());
idb.addValue(sv, "iod-hours", hours);
if(hours < 4)
idb.addValue(sv, "eph-disco", disco);
if(0 && hours < 2) {
ofstream orbitcsv("orbit."+to_string(ent.first)+"."+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 * 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";
}
}
ent.second.clearPrev();
}
}
}
#if 0
else if(ubxClass == 0x01 && ubxType == 0x01) { // POSECF
struct pos
{
uint32_t iTOW;
int32_t ecefX; // cm
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
};
pos p;
memcpy(&p, msg.c_str(), sizeof(pos));
cout<<"Position: ("<< p.ecefX / 100000.0<<", "
<< p.ecefY / 100000.0<<", "
<< p.ecefZ / 100000.0<<") +- "<<p.pAcc<<" cm"<<endl;
g_ourpos.x = p.ecefX/100.0;
g_ourpos.y = p.ecefY/100.0;
g_ourpos.z = p.ecefZ/100.0;
}
else if(ubxClass == 0x02 && ubxType == 0x15) { // RAWX
cout<<"Got "<<(int)msg[11] <<" measurements "<<endl;
double rcvTow;
memcpy(&rcvTow, &msg[0], 8);
for(int n=0 ; n < msg[11]; ++n) {
double prMes;
double cpMes;
float doppler;
memcpy(&prMes, &msg[16+32*n], 8);
memcpy(&cpMes, &msg[24+32*n], 8);
memcpy(&doppler, &msg[32+32*n], 4);
int gnssid = msg[36+32*n];
int sv = msg[37+32*n];
uint16_t locktimems;
memcpy(&locktimems, &msg[40+32*n], 2);
uint8_t prStddev = msg[43+23*n] & 0xf;
uint8_t cpStddev = msg[44+23*n] & 0xf;
uint8_t doStddev = msg[45+23*n] & 0xf;
uint8_t trkStat = msg[46+23*n] & 0xf;
if(gnssid ==2 && g_svstats[sv].completeIOD()) {
Point sat;
Point us=g_ourpos;
getCoordinates(g_svstats[sv].wn, rcvTow, g_svstats[sv].liveIOD(), &sat);
Point core;
Vector us2sat(us, sat);
Vector speed;
getSpeed(g_svstats[sv].wn, rcvTow, g_svstats[sv].liveIOD(), &speed);
cout<<sv<<" radius: "<<Vector(core, sat).length()<<", distance: "<<us2sat.length()<<", orbital velocity: "<<speed.length()/1000.0<<" km/s, ";
Vector core2us(core, us);
Vector dx(us, sat); // = 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);
cout <<"elev: "<<90 - deg<< " ("<<g_svstats[sv].el<<")\n";
us2sat.norm();
double radvel=us2sat.inner(speed);
double c=299792458;
double galileol1f = 1575.42 * 1000000; // frequency
double preddop = -galileol1f*radvel/c;
double ephage = ephAge(g_svstats[sv].tow, g_svstats[sv].liveIOD().t0e*60);
cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "<<doppler<<endl;
dopplercsv << std::fixed << utcFromGST(g_svstats[sv].wn, rcvTow) <<" " <<gnssid <<" " <<sv<<" "<<prMes<<" "<<cpMes <<" " << doppler<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " <<locktimems<<" " <<ephage << " " << ldexp(0.01, prStddev) << " " << cpStddev *0.4 <<" " <<
ldexp(0.002, doStddev) <<" " << (unsigned int)trkStat << endl;
}
}
}
else
cout << "ubxClass: "<<(unsigned int)ubxClass<<", ubxType: "<<(unsigned int)ubxType<<", size="<<msg.size()<<endl;
#endif
}
}
catch(EofException& e)
{}
}
catch(std::exception& e)
{
cerr<<"Exiting because of fatal error "<<e.what()<<endl;
}

29
ubx.cc
View File

@ -2,6 +2,7 @@
#include "ubx.hh"
#include "fmt/format.h"
#include "fmt/printf.h"
#include "bits.hh"
using namespace std;
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
@ -58,3 +59,31 @@ std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, st
*/
return msg;
}
basic_string<uint8_t> getInavFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
{
// byte order adjustment
std::basic_string<uint8_t> payload;
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
for(int j=1; j <= 4; ++j)
payload.append(1, msg[8 + (i+1) * 4 -j]);
/* test crc (4(pad) + 114 + 82 bits) */
unsigned char crc_buff[26]={0};
unsigned int i,j;
for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8));
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8));
if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) {
cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) <<endl;
throw CRCMismatch();
}
std::basic_string<uint8_t> inav;
for (i=0,j=2; i<14; i++, j+=8)
inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
for (i=0,j=2; i< 2; i++, j+=8)
inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8));
return inav;
}

4
ubx.hh
View File

@ -4,3 +4,7 @@ uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_vi
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str);
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& str);
std::basic_string<uint8_t> getInavFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
struct CRCMismatch{};

413
ubxparse.cc
View File

@ -47,20 +47,24 @@ Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000);
int g_tow, g_wn, g_dtLS{18};
/* inav schedule:
1) Find plausible start time of next cycle
Current cycle: TOW - (TOW%30)
Next cycle: TOW - (TOW%30) + 30
t n w
0 1: 2 wn % 30 == 0
2 2: 4 wn % 30 == 2
4 3: 6
6 4: 7/9
8 5: 8/10
10 6: 0 WN/TOW
0 1: 2 wn % 30 == 0
2 2: 4 wn % 30 == 2
4 3: 6 WN/TOW 4 -> set startTow, startTowFresh
6 4: 7/9
8 5: 8/10
10 6: 0 TOW
12 7: 0 WN/TOW
14 8: 0 WN/TOW
16 9: 0 WN/TOW
18 10: 0 WN/TOW
20 11: 1
20 11: 1
22 12: 3
24 13: 5
24 13: 5 WN/TOW
26 14: 0 WN/TOW
28 15: 0 WN/TOW
*/
@ -550,14 +554,22 @@ try
nlohmann::json ret = nlohmann::json::object();
ret["leap-seconds"] = g_dtLS;
map<int, int> utcstats;
map<int, int> utcstats, gpsgststats;
for(const auto& s: g_svstats) {
// XXX this might run BEFORE we have gps/utc stats!
int dw = (uint8_t)g_wn - s.second.wn0t;
int age = dw * 7 * 86400 + g_tow - s.second.t0t * 3600;
utcstats[age]=s.first;
uint8_t wn0g = s.second.wn0t;
int dwg = (((uint8_t)g_wn)&(1+2+4+8+16+32)) - wn0g;
age = dwg*7*86400 + g_tow - s.second.t0g * 3600;
gpsgststats[age]=s.first;
}
if(utcstats.empty()) {
ret["utc-offset"]=nullptr;
ret["utc-offset-ns"]=nullptr;
}
else {
int sv = utcstats.begin()->second; // freshest SV
@ -566,6 +578,18 @@ try
ret["leap-second-planned"] = (g_svstats[sv].dtLSF != g_svstats[sv].dtLS);
}
if(gpsgststats.empty()) {
ret["gps-offset-ns"]=nullptr;
}
else {
int sv = gpsgststats.begin()->second; // freshest SV
long shift = g_svstats[sv].a0g * (1U<<16) + g_svstats[sv].a1g * utcstats.begin()->first; // in 2^-51 seconds units
ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21);
}
return ret;
});
@ -657,7 +681,7 @@ try
clockcsv <<"timestamp sv af0 af1 af2 t0c age offset"<<endl;
ofstream dopplercsv("doppler.csv");
dopplercsv<<"timestamp gnssid sv prmes cpmes doppler preddop distance radvel locktimems iod_age"<<endl;
dopplercsv<<"timestamp gnssid sv prmes cpmes doppler preddop distance radvel locktimems iod_age prstd cpstd dostd trkstat"<<endl;
string line;
@ -759,216 +783,169 @@ try
if(msg[0] != 2) // galileo
continue;
unsigned int sv = (int)msg[1];
// cout << "Word type "<< (int)(msg[11] & (~(64+128))) <<" SV " << (int)msg[1]<<"\n";
// for(unsigned int n = 8; n < msg.size() ; ++n) {
// fmt::printf("%02x ", msg[n]);
// }
// cout<<"\n";
std::basic_string<uint8_t> payload;
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
for(int j=1; j <= 4; ++j)
payload.append(1, msg[8 + (i+1) * 4 -j]);
try {
unsigned int sv = (int)msg[1];
basic_string<uint8_t> inav = getInavFromSFRBXMsg(msg);
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
// for(auto& c : inav)
// fmt::printf("%02x ", c);
unsigned int wtype = getbitu(&inav[0], 0, 6);
g_svstats[sv].addWord(inav);
if(g_svstats[sv].e1bhs || g_svstats[sv].e5bhs || g_svstats[sv].e1bdvs || g_svstats[sv].e5bdvs) {
if(sv != 18 && sv != 14)
cout << "sv "<<sv<<" health: " << g_svstats[sv].e1bhs <<" " << g_svstats[sv].e5bhs <<" " << g_svstats[sv].e1bdvs <<" "<< g_svstats[sv].e5bdvs <<endl;
}
if(!wtype) {
if(getbitu(&inav[0], 6,2) == 2) {
// wn = getbitu(&inav[0], 96, 12);
// tow = getbitu(&inav[0], 108, 20);
}
}
else if(wtype >=1 && wtype <= 4) { // ephemeris
uint16_t iod = getbitu(&inav[0], 6, 10);
if(wtype == 1 && g_tow) {
}
else if(wtype == 3) {
/* test crc (4(pad) + 114 + 82 bits) */
unsigned char crc_buff[26]={0};
unsigned int i,j;
for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8));
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8));
if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) {
cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) <<endl;
continue;
}
// for(auto& c : payload)
// fmt::printf("%02x ", c);
idb.addValue(sv, "sisa", g_svstats[sv].iods[iod].sisa);
}
else if(wtype == 4) {
// cout<<"\n";
std::basic_string<uint8_t> inav;
for (i=0,j=2; i<14; i++, j+=8)
inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
for (i=0,j=2; i< 2; i++, j+=8)
inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8));
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
// for(auto& c : inav)
// fmt::printf("%02x ", c);
unsigned int wtype = getbitu(&inav[0], 0, 6);
g_svstats[sv].addWord(inav);
if(g_svstats[sv].e1bhs || g_svstats[sv].e5bhs || g_svstats[sv].e1bdvs || g_svstats[sv].e5bdvs) {
if(sv != 18 && sv != 14)
cout << "sv "<<sv<<" health: " << g_svstats[sv].e1bhs <<" " << g_svstats[sv].e5bhs <<" " << g_svstats[sv].e1bdvs <<" "<< g_svstats[sv].e5bdvs <<endl;
}
if(!wtype) {
if(getbitu(&inav[0], 6,2) == 2) {
// wn = getbitu(&inav[0], 96, 12);
// tow = getbitu(&inav[0], 108, 20);
}
}
else if(wtype >=1 && wtype <= 4) { // ephemeris
uint16_t iod = getbitu(&inav[0], 6, 10);
if(wtype == 1 && g_tow) {
// int t0e = 60*getbitu(&inav[0], 16, 14);
// int age = (tow - t0e)/60;
// uint32_t e = getbitu(&inav[0], 6+10+14+32, 32);
}
else if(wtype == 3) {
// unsigned int sisa = getbitu(&inav[0], 120, 8);
idb.addValue(sv, "sisa", g_svstats[sv].iods[iod].sisa);
}
else if(wtype == 4) {
idb.addValue(sv, "af0", g_svstats[sv].iods[iod].af0);
idb.addValue(sv, "af1", g_svstats[sv].iods[iod].af1);
idb.addValue(sv, "af2", g_svstats[sv].iods[iod].af2);
idb.addValue(sv, "t0c", g_svstats[sv].iods[iod].t0c);
double age = ephAge(g_tow, g_svstats[sv].iods[iod].t0c * 60);
/*
cout<<"Atomic age "<<age<<", g_tow: "<<g_tow<<", t0c*60 = "<<g_svstats[sv].iods[iod].t0c * 60<<endl;
cout<<"af0: "<<g_svstats[sv].iods[iod].af0<<", af1: "<<g_svstats[sv].iods[iod].af1 << ", af2: "<<(int)g_svstats[sv].iods[iod].af2<<endl;
*/
double offset = ldexp(1000.0*(1.0*g_svstats[sv].iods[iod].af0 + ldexp(age*g_svstats[sv].iods[iod].af1, -12)), -34);
// XXX what units is this in then? milliseconds?
/*
cout <<"Atomic offset: "<<offset<<endl;
cout << 1.0*g_svstats[sv].iods[iod].af0/(1LL<<34) << endl;
cout << age * (g_svstats[sv].iods[iod].af1/(1LL<<46)) << endl;
*/
idb.addValue(sv, "atomic_offset_ns", 1000000.0*offset);
}
else
;
}
else if(wtype == 5) {
idb.addValue(sv, "ai0", g_svstats[sv].ai0);
idb.addValue(sv, "ai1", g_svstats[sv].ai1);
idb.addValue(sv, "ai2", g_svstats[sv].ai2);
idb.addValue(sv, "sf1", g_svstats[sv].sf1);
idb.addValue(sv, "sf2", g_svstats[sv].sf2);
idb.addValue(sv, "sf3", g_svstats[sv].sf3);
idb.addValue(sv, "sf4", g_svstats[sv].sf4);
idb.addValue(sv, "sf5", g_svstats[sv].sf5);
idb.addValue(sv, "BGDE1E5a", g_svstats[sv].BGDE1E5a);
idb.addValue(sv, "BGDE1E5b", g_svstats[sv].BGDE1E5b);
idb.addValue(sv, "e1bhs", g_svstats[sv].e1bhs);
idb.addValue(sv, "e5bhs", g_svstats[sv].e5bhs);
idb.addValue(sv, "e5bdvs", g_svstats[sv].e5bdvs);
idb.addValue(sv, "e1bdvs", g_svstats[sv].e1bdvs);
}
else if(wtype == 6) { // GST-UTC
idb.addValue(sv, "a0", g_svstats[sv].a0);
idb.addValue(sv, "a1", g_svstats[sv].a1);
g_dtLS = g_svstats[sv].dtLS;
int dw = (uint8_t)g_wn - g_svstats[sv].wn0t;
if(g_tow && g_wn) {
int age = dw * 7 * 86400 + g_tow - g_svstats[sv].t0t * 3600;
// a0 = 2^-30 s, a1 = 2^-50 s/s
long shift = g_svstats[sv].a0 * (1LL<<20) + g_svstats[sv].a1 * age; // in 2^-50 seconds units
time_t t = utcFromGST(g_wn, g_tow);
gstutc << t << " " << sv <<" " << g_tow << " " << (g_svstats[sv].t0t * 3600) << " "<< age <<" " <<shift << " " << shift * pow(2, -50) * 1000000000 << " " << g_svstats[sv].a0 <<" " << g_svstats[sv].a1 << "\n";
// 2^-30
idb.addValue(sv, "utc_diff_ns", 1.073741824*ldexp(shift, -20));
// cout<<humanTime(g_wn, g_tow)<<" sv "<<sv<< " GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1)+", age="+to_string(age/3600)+"h, dw="+to_string(dw)
// +", wn0t="+to_string(wn0t)+", wn8="+to_string(g_wn&0xff)+"\n";
}
}
else if(wtype == 10) { // GSTT GPS
idb.addValue(sv, "a0g", g_svstats[sv].a0g);
idb.addValue(sv, "a1g", g_svstats[sv].a1g);
int a0g = getbits(&inav[0], 86, 16);
int a1g = getbits(&inav[0], 102, 12);
int t0g = getbitu(&inav[0], 114, 8);
// uint8_t wn0g = getbitu(&inav[0], 122, 6);
// int dw = (((uint8_t)g_wn)&(1+2+4+8+16+32)) - wn0g;
if(g_tow && g_wn) {
int age = g_tow - t0g * 3600; // xx WHERE IS DW??
// a0g = 2^-32 s, a1 = 2^-50 s/s
int shift = a0g * (1U<<16) + a1g * age; // in 2^-51 seconds units
time_t t = utcFromGST(g_wn, g_tow);
gstgps << t << " " << sv <<" " << g_tow << " " << (t0g * 3600) <<" " << age <<" " <<shift << " " << shift * pow(2, -50) * 1000000000 << " " << a0g <<" " << a1g << "\n";
// cout<<humanTime(g_wn, g_tow)<<" sv "<<sv<< " GST-GPS, a0g="+to_string(a0g)+", a1g="+to_string(a1g)+", t0g="+to_string(t0g)+", age="+to_string(g_tow/3600-t0g)+"h, dw="+to_string(dw)
// +", wn0g="+to_string(wn0g)+", wn6="+to_string(g_wn&(1+2+4+8+16+32))+"\n";
}
}
for(auto& ent : g_svstats) {
// fmt::printf("%2d\t", ent.first);
if(ent.second.completeIOD() && ent.second.prevIOD.first >= 0) {
time_t t = utcFromGST(g_wn, g_tow);
sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl;
// cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n";
double clockage = ephAge(g_tow, ent.second.liveIOD().t0c * 60);
double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46);
clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + g_wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset<<endl;
int ephage = ephAge(g_tow, ent.second.prevIOD.second.t0e * 60);
if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) {
cout<<humanTime(g_wn, g_tow)<<" sv "<<ent.first<<" 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;
idb.addValue(sv, "af0", g_svstats[sv].iods[iod].af0);
idb.addValue(sv, "af1", g_svstats[sv].iods[iod].af1);
idb.addValue(sv, "af2", g_svstats[sv].iods[iod].af2);
idb.addValue(sv, "t0c", g_svstats[sv].iods[iod].t0c);
double age = ephAge(g_tow, g_svstats[sv].iods[iod].t0c * 60);
/*
cout<<"Atomic age "<<age<<", g_tow: "<<g_tow<<", t0c*60 = "<<g_svstats[sv].iods[iod].t0c * 60<<endl;
cout<<"af0: "<<g_svstats[sv].iods[iod].af0<<", af1: "<<g_svstats[sv].iods[iod].af1 << ", af2: "<<(int)g_svstats[sv].iods[iod].af2<<endl;
*/
double offset = ldexp(1000.0*(1.0*g_svstats[sv].iods[iod].af0 + ldexp(age*g_svstats[sv].iods[iod].af1, -12)), -34);
// XXX what units is this in then? milliseconds?
/*
cout <<"Atomic offset: "<<offset<<endl;
cout << 1.0*g_svstats[sv].iods[iod].af0/(1LL<<34) << endl;
cout << age * (g_svstats[sv].iods[iod].af1/(1LL<<46)) << endl;
*/
idb.addValue(sv, "atomic_offset_ns", 1000000.0*offset);
}
Point p, oldp;
getCoordinates(g_wn, g_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(g_wn, g_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<<ent.first<<" discontinuity after "<< hours<<" hours: "<< disco <<endl;
idb.addValue(sv, "iod-actual", ent.second.getIOD());
idb.addValue(sv, "iod-hours", hours);
else
;
}
else if(wtype == 5) {
idb.addValue(sv, "ai0", g_svstats[sv].ai0);
idb.addValue(sv, "ai1", g_svstats[sv].ai1);
idb.addValue(sv, "ai2", g_svstats[sv].ai2);
if(hours < 4)
idb.addValue(sv, "eph-disco", disco);
if(0 && hours < 2) {
ofstream orbitcsv("orbit."+to_string(ent.first)+"."+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 * 60 + offset;
Point p, oldp;
getCoordinates(g_wn, t, ent.second.liveIOD(), &p);
getCoordinates(g_wn, t, ent.second.prevIOD.second, &oldp);
time_t posix = utcFromGST(g_wn, t);
orbitcsv << posix <<" "
<<p.x<<" " <<p.y<<" "<<p.z<<" "
<<oldp.x<<" " <<oldp.y<<" "<<oldp.z<<"\n";
idb.addValue(sv, "sf1", g_svstats[sv].sf1);
idb.addValue(sv, "sf2", g_svstats[sv].sf2);
idb.addValue(sv, "sf3", g_svstats[sv].sf3);
idb.addValue(sv, "sf4", g_svstats[sv].sf4);
idb.addValue(sv, "sf5", g_svstats[sv].sf5);
idb.addValue(sv, "BGDE1E5a", g_svstats[sv].BGDE1E5a);
idb.addValue(sv, "BGDE1E5b", g_svstats[sv].BGDE1E5b);
idb.addValue(sv, "e1bhs", g_svstats[sv].e1bhs);
idb.addValue(sv, "e5bhs", g_svstats[sv].e5bhs);
idb.addValue(sv, "e5bdvs", g_svstats[sv].e5bdvs);
idb.addValue(sv, "e1bdvs", g_svstats[sv].e1bdvs);
}
else if(wtype == 6) { // GST-UTC
idb.addValue(sv, "a0", g_svstats[sv].a0);
idb.addValue(sv, "a1", g_svstats[sv].a1);
g_dtLS = g_svstats[sv].dtLS;
int dw = (uint8_t)g_wn - g_svstats[sv].wn0t;
if(g_tow && g_wn) {
int age = dw * 7 * 86400 + g_tow - g_svstats[sv].t0t * 3600;
// a0 = 2^-30 s, a1 = 2^-50 s/s
long shift = g_svstats[sv].a0 * (1LL<<20) + g_svstats[sv].a1 * age; // in 2^-50 seconds units
time_t t = utcFromGST(g_wn, g_tow);
gstutc << t << " " << sv <<" " << g_tow << " " << (g_svstats[sv].t0t * 3600) << " "<< age <<" " <<shift << " " << shift * pow(2, -50) * 1000000000 << " " << g_svstats[sv].a0 <<" " << g_svstats[sv].a1 << "\n";
// 2^-30
idb.addValue(sv, "utc_diff_ns", 1.073741824*ldexp(shift, -20));
// cout<<humanTime(g_wn, g_tow)<<" sv "<<sv<< " GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1)+", age="+to_string(age/3600)+"h, dw="+to_string(dw)
// +", wn0t="+to_string(wn0t)+", wn8="+to_string(g_wn&0xff)+"\n";
}
}
else if(wtype == 10) { // GSTT GPS
idb.addValue(sv, "a0g", g_svstats[sv].a0g);
idb.addValue(sv, "a1g", g_svstats[sv].a1g);
idb.addValue(sv, "t0g", g_svstats[sv].t0g);
}
for(auto& ent : g_svstats) {
// fmt::printf("%2d\t", ent.first);
if(ent.second.completeIOD() && ent.second.prevIOD.first >= 0) {
time_t t = utcFromGST(g_wn, g_tow);
sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl;
// cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n";
double clockage = ephAge(g_tow, ent.second.liveIOD().t0c * 60);
double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46);
clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + g_wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset<<endl;
int ephage = ephAge(g_tow, ent.second.prevIOD.second.t0e * 60);
if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) {
cout<<humanTime(g_wn, g_tow)<<" sv "<<ent.first<<" 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(g_wn, g_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(g_wn, g_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<<ent.first<<" discontinuity after "<< hours<<" hours: "<< disco <<endl;
idb.addValue(sv, "iod-actual", ent.second.getIOD());
idb.addValue(sv, "iod-hours", hours);
if(hours < 4)
idb.addValue(sv, "eph-disco", disco);
if(0 && hours < 2) {
ofstream orbitcsv("orbit."+to_string(ent.first)+"."+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 * 60 + offset;
Point p, oldp;
getCoordinates(g_wn, t, ent.second.liveIOD(), &p);
getCoordinates(g_wn, t, ent.second.prevIOD.second, &oldp);
time_t posix = utcFromGST(g_wn, t);
orbitcsv << posix <<" "
<<p.x<<" " <<p.y<<" "<<p.z<<" "
<<oldp.x<<" " <<oldp.y<<" "<<oldp.z<<"\n";
}
}
ent.second.clearPrev();
}
ent.second.clearPrev();
}
}
catch(CRCMismatch& e) {
cout<<"Had a CRC mismatch parsing an INAV"<<endl;
continue;
}
}
else if (ubxClass == 1 && ubxType == 0x35) { // UBX-NAV-SAT
cout<< "Info for "<<(int) msg[5]<<" svs: \n";
@ -1051,12 +1028,15 @@ try
memcpy(&prMes, &msg[16+32*n], 8);
memcpy(&cpMes, &msg[24+32*n], 8);
memcpy(&doppler, &msg[32+32*n], 4);
int gnssid = msg[36+32*n];
int sv = msg[37+32*n];
uint16_t locktimems;
memcpy(&locktimems, &msg[40+32*n], 2);
uint8_t prStddev = msg[43+23*n] & 0xf;
uint8_t cpStddev = msg[44+23*n] & 0xf;
uint8_t doStddev = msg[45+23*n] & 0xf;
uint8_t trkStat = msg[46+23*n] & 0xf;
if(gnssid ==2 && g_svstats[sv].completeIOD()) {
Point sat;
Point us=g_ourpos;
@ -1082,7 +1062,8 @@ try
double ephage = ephAge(g_tow, g_svstats[sv].liveIOD().t0e*60);
cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "<<doppler<<endl;
dopplercsv << std::fixed << utcFromGST(g_wn, rcvTow) <<" " <<gnssid <<" " <<sv<<" "<<prMes<<" "<<cpMes <<" " << doppler<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " <<locktimems<<" " <<ephage <<endl;
dopplercsv << std::fixed << utcFromGST(g_wn, rcvTow) <<" " <<gnssid <<" " <<sv<<" "<<prMes<<" "<<cpMes <<" " << doppler<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " <<locktimems<<" " <<ephage << " " << ldexp(0.01, prStddev) << " " << cpStddev *0.4 <<" " <<
ldexp(0.002, doStddev) <<" " << (unsigned int)trkStat << endl;
}
}
}

524
ubxtool.cc
View File

@ -16,6 +16,12 @@
#include <string.h>
#include "fmt/format.h"
#include "fmt/printf.h"
#include "bits.hh"
#include "galileo.hh"
#include <arpa/inet.h>
#include "navmon.pb.h"
struct timespec g_gstutc;
uint16_t g_wn;
using namespace std;
#define BAUDRATE B921600
@ -67,10 +73,57 @@ size_t writen2(int fd, const void *buf, size_t count)
return count;
}
/* inav schedule:
1) Find plausible start time of next cycle
Current cycle: TOW - (TOW%30)
Next cycle: TOW - (TOW%30) + 30
t n w
0 1: 2 wn % 30 == 0
2 2: 4 wn % 30 == 2
4 3: 6 WN/TOW 4 -> set startTow, startTowFresh
6 4: 7/9
8 5: 8/10
10 6: 0 TOW
12 7: 0 WN/TOW
14 8: 0 WN/TOW
16 9: 0 WN/TOW
18 10: 0 WN/TOW
20 11: 1
22 12: 3
24 13: 5 WN/TOW
26 14: 0 WN/TOW
28 15: 0 WN/TOW
*/
/*
if(ubxClass == 2 && ubxType == 89) { // SAR
string hexstring;
for(int n = 0; n < 15; ++n)
hexstring+=fmt::format("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4));
// int sv = (int)msg[2];
// wk.emitLine(sv, "SAR "+hexstring);
// cout<<"SAR: sv = "<< (int)msg[2] <<" ";
// for(int n=4; n < 12; ++n)
// fmt::printf("%02x", (int)msg[n]);
// for(int n = 0; n < 15; ++n)
// fmt::printf("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4));
// cout << " Type: "<< (int) msg[12] <<"\n";
// cout<<"Parameter: (len = "<<msg.length()<<") ";
// for(unsigned int n = 13; n < msg.length(); ++n)
// fmt::printf("%02x ", (int)msg[n]);
// cout<<"\n";
}
*/
class UBXMessage
{
public:
struct BadChecksum{};
explicit UBXMessage(basic_string_view<uint8_t> src)
{
d_raw = src;
@ -79,7 +132,7 @@ public:
uint16_t csum = calcUbxChecksum(getClass(), getType(), d_raw.substr(6, d_raw.size()-8));
if(csum != d_raw.at(d_raw.size()-2) + 256*d_raw.at(d_raw.size()-1))
throw std::runtime_error("Bad UBX checksum");
throw BadChecksum();
}
uint8_t getClass() const
@ -97,7 +150,7 @@ public:
std::basic_string<uint8_t> d_raw;
};
std::pair<struct timeval, UBXMessage> getUBXMessage(int fd)
std::pair<UBXMessage, struct timeval> getUBXMessage(int fd)
{
uint8_t marker[2]={0};
for(;;) {
@ -123,7 +176,7 @@ std::pair<struct timeval, UBXMessage> getUBXMessage(int fd)
res=readn2(fd, buffer, len+2);
msg.append(buffer, len+2); // checksum
return make_pair(tv, UBXMessage(msg));
return make_pair(UBXMessage(msg), tv);
}
}
}
@ -131,7 +184,8 @@ std::pair<struct timeval, UBXMessage> getUBXMessage(int fd)
UBXMessage waitForUBX(int fd, int seconds, uint8_t ubxClass, uint8_t ubxType)
{
for(int n=0; n < seconds*20; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
auto [msg, tv] = getUBXMessage(fd);
(void) tv;
// cerr<<"Got: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
if(msg.getClass() == ubxClass && msg.getType() == ubxType) {
return msg;
@ -143,7 +197,8 @@ UBXMessage waitForUBX(int fd, int seconds, uint8_t ubxClass, uint8_t ubxType)
bool waitForUBXAckNack(int fd, int seconds)
{
for(int n=0; n < seconds*4; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
auto [msg, tv] = getUBXMessage(fd);
(void)tv;
// cerr<<"Got: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
if(msg.getClass() == 0x05 && msg.getType() == 0x01) {
return true;
@ -155,6 +210,16 @@ bool waitForUBXAckNack(int fd, int seconds)
throw std::runtime_error("Did not get ACK/NACK response on time");
}
void emitNMM(int fd, const NavMonMessage& nmm)
{
string out;
nmm.SerializeToString(& out);
writen2(fd, "bert", 4);
uint16_t len = htons(out.size());
writen2(fd, &len, 2);
writen2(fd, out.c_str(), out.size());
}
void enableUBXMessageUSB(int fd, uint8_t ubxClass, uint8_t ubxType)
{
@ -168,157 +233,338 @@ void enableUBXMessageUSB(int fd, uint8_t ubxClass, uint8_t ubxType)
int main(int argc, char** argv)
{
int fd;
struct termios oldtio,newtio;
ofstream orbitcsv("orbit.csv");
orbitcsv<<"timestamp gnssid sv prmes cpmes doppler"<<endl;
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
if (fd <0) {perror(MODEMDEVICE); exit(-1); }
tcgetattr(fd,&oldtio); /* save current port settings */
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0; /* inter-character timer unused */
newtio.c_cc[VMIN] = 5; /* blocking read until 5 chars received */
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
for(int n=0; n < 5; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
cerr<<"Read some init: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
// if(msg.getClass() == 0x2)
//cerr<<string((char*)msg.getPayload().c_str(), msg.getPayload().size()) <<endl;
int fd;
bool fromFile=false;
if(argc > 1) {
fromFile = true;
fd = open(argv[1], O_RDONLY );
}
std::basic_string<uint8_t> msg;
if(argc> 1 && string(argv[1])=="cold") {
cerr<<"Sending cold start!"<<endl;
msg = buildUbxMessage(0x06, 0x04, {0xff, 0xff, 0x04, 0x00}); // cold start!
else
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
if (fd <0) {perror(MODEMDEVICE); exit(-1); }
if(!fromFile) {
tcgetattr(fd,&oldtio); /* save current port settings */
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0; /* inter-character timer unused */
newtio.c_cc[VMIN] = 5; /* blocking read until 5 chars received */
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
for(int n=0; n < 5; ++n) {
auto [msg, timestamp] = getUBXMessage(fd);
(void)timestamp;
cerr<<"Read some init: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
// if(msg.getClass() == 0x2)
//cerr<<string((char*)msg.getPayload().c_str(), msg.getPayload().size()) <<endl;
}
std::basic_string<uint8_t> msg;
cerr<<"Asking for rate"<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89}); // ask for rates of class 0x02 type 89, RLM
writen2(fd, msg.c_str(), msg.size());
for(int n=0; n < 10; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
cerr<<"Read some init: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
}
}
cerr<<"Asking for rate"<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89}); // ask for rates of class 0x02 type 89, RLM
writen2(fd, msg.c_str(), msg.size());
UBXMessage um=waitForUBX(fd, 2, 0x06, 0x01);
cerr<<"Message rate: "<<endl;
for(const auto& c : um.getPayload())
cerr<<(int)c<< " ";
cerr<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89, 0, 0, 0, 1, 0, 0});
writen2(fd, msg.c_str(), msg.size());
if(waitForUBXAckNack(fd, 2))
cerr<<"Got ack on rate setting"<<endl;
else
cerr<<"Got nack on rate setting"<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89});
writen2(fd, msg.c_str(), msg.size());
um=waitForUBX(fd, 2, 0x06, 0x01); // ignore
cerr<<"Disabling NMEA"<<endl;
msg = buildUbxMessage(0x06, 0x00, {0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x01,0x00,0x00,0x00,0x00,0x00});
writen2(fd, msg.c_str(), msg.size());
if(waitForUBXAckNack(fd, 2))
cerr<<"NMEA disabled"<<endl;
else
cerr<<"Got NACK disabling NMEA"<<endl;
msg = buildUbxMessage(0x06, 0x00, {0x03});
writen2(fd, msg.c_str(), msg.size());
um=waitForUBX(fd, 2, 0x06, 0x00);
cerr<<"Protocol settings on USB: \n";
for(const auto& c : um.getPayload())
cerr<<(int)c<< " ";
cerr<<endl;
cerr<<"Enabling UBX-RXM-RAWX"<<endl;
enableUBXMessageUSB(fd, 0x02, 0x15);
UBXMessage um=waitForUBX(fd, 2, 0x06, 0x01);
cerr<<"Message rate: "<<endl;
for(const auto& c : um.getPayload())
cerr<<(int)c<< " ";
cerr<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89, 0, 0, 0, 1, 0, 0});
writen2(fd, msg.c_str(), msg.size());
if(waitForUBXAckNack(fd, 2))
cerr<<"Got ack on rate setting"<<endl;
else
cerr<<"Got nack on rate setting"<<endl;
msg = buildUbxMessage(0x06, 0x01, {0x02, 89});
writen2(fd, msg.c_str(), msg.size());
um=waitForUBX(fd, 2, 0x06, 0x01); // ignore
cerr<<"Disabling NMEA"<<endl;
msg = buildUbxMessage(0x06, 0x00, {0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x01,0x00,0x00,0x00,0x00,0x00});
writen2(fd, msg.c_str(), msg.size());
if(waitForUBXAckNack(fd, 2))
cerr<<"NMEA disabled"<<endl;
else
cerr<<"Got NACK disabling NMEA"<<endl;
msg = buildUbxMessage(0x06, 0x00, {0x03});
writen2(fd, msg.c_str(), msg.size());
um=waitForUBX(fd, 2, 0x06, 0x00);
cerr<<"Protocol settings on USB: \n";
for(const auto& c : um.getPayload())
cerr<<(int)c<< " ";
cerr<<endl;
cerr<<"Enabling UBX-RXM-RAWX"<<endl;
enableUBXMessageUSB(fd, 0x02, 0x15);
cerr<<"Enabling UBX-RXM-SFRBX"<<endl;
enableUBXMessageUSB(fd, 0x02, 0x13);
cerr<<"Enabling UBX-NAV-POSECEF"<<endl;
enableUBXMessageUSB(fd, 0x01, 0x01);
cerr<<"Enabling UBX-NAV-SAT"<<endl;
enableUBXMessageUSB(fd, 0x01, 0x35);
enableUBXMessageUSB(fd, 0x02, 0x13);
cerr<<"Enabling UBX-NAV-POSECEF"<<endl;
enableUBXMessageUSB(fd, 0x01, 0x01);
cerr<<"Enabling UBX-NAV-SAT"<<endl;
enableUBXMessageUSB(fd, 0x01, 0x35);
cerr<<"Enabling UBX-NAV-PVT"<<endl; // position, velocity, time fix
enableUBXMessageUSB(fd, 0x01, 0x07);
}
/* goal: isolate UBX messages, ignore everyting else.
The challenge is that we might sometimes hit the 0xb5 0x62 marker
in the middle of a message, which will cause us to possibly jump over valid messages */
in the middle of a message, which will cause us to possibly jump over valid messages
This program sits on the serial link and therefore has the best timestamps.
At least Galileo messages sometimes rely on the timestamp-of-receipt, but the promise
is that such messages are sent at whole second intervals.
We therefore perform a layering violation and peer into the message to see
what timestamp it claims to have, so that we can set subsequent timestamps correctly.
*/
std::map<pair<int,int>, struct timeval> lasttv, tv;
unsigned int nextCycleTOW{0};
unsigned int curCycleTOW{0};
bool curCycleTOWFresh=false;
for(;;) {
auto [timestamp, msg] = getUBXMessage(fd);
auto payload = msg.getPayload();
try {
auto [msg, timestamp] = getUBXMessage(fd);
(void)timestamp;
auto payload = msg.getPayload();
// should turn this into protobuf
if(msg.getClass() == 0x01 && msg.getType() == 0x07) { // UBX-NAV-PVT
struct PVT
{
uint32_t itow;
uint16_t year;
uint8_t month; // jan = 1
uint8_t day;
uint8_t hour; // 24
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fixtype;
} __attribute__((packed));
PVT pvt;
memcpy(&pvt, &payload[0], sizeof(pvt));
struct tm tm;
memset(&tm, 0, sizeof(tm));
tm.tm_year = pvt.year - 1900;
tm.tm_mon = pvt.month - 1;
tm.tm_mday = pvt.day;
tm.tm_hour = pvt.hour;
tm.tm_min = pvt.min;
tm.tm_sec = pvt.sec;
if(msg.getClass() == 0x01 && msg.getType() == 0x01) { // POSECF
struct pos
{
uint32_t iTOW;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
};
pos p;
memcpy(&p, payload.c_str(), sizeof(pos));
cerr<<"Position: ("<< p.ecefX / 100000.0<<", "
<< p.ecefY / 100000.0<<", "
<< p.ecefZ / 100000.0<<") +- "<<p.pAcc<<" cm"<<endl;
}
if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX
pair<int,int> id = make_pair(payload[0], payload[1]);
tv[id] = timestamp;
if(lasttv.count(id)) {
fmt::fprintf(stderr, "gnssid %d sv %d, %d:%d -> %d:%d, delta=%d\n",
payload[0], payload[1], lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec);
uint32_t satt = timegm(&tm);
double satutc = timegm(&tm) + pvt.nano/1000000000.0; // negative is no problem here
if(pvt.nano < 0) {
pvt.sec--;
satt--;
pvt.nano += 1000000000;
}
g_gstutc.tv_sec = satt;
g_gstutc.tv_nsec = pvt.nano;
double seconds= pvt.sec + pvt.nano/1000000000.0;
fmt::fprintf(stderr, "Satellite UTC: %02d:%02d:%06.4f -> %.4f or %d:%f\n", tm.tm_hour, tm.tm_min, seconds, satutc, timegm(&tm), pvt.nano/1000.0);
if(!fromFile) {
struct tm ourtime;
time_t ourt = timestamp.tv_sec;
gmtime_r(&ourt, &ourtime);
double ourutc = ourt + timestamp.tv_usec/1000000.0;
seconds = ourtime.tm_sec + timestamp.tv_usec/1000000.0;
fmt::fprintf(stderr, "Our UTC : %02d:%02d:%06.4f -> %.4f or %d:%f -> delta = %.4fs\n", tm.tm_hour, tm.tm_min, seconds, ourutc, timestamp.tv_sec, 1.0*timestamp.tv_usec, ourutc - satutc);
}
}
lasttv[id]=tv[id];
}
writen2(1, msg.d_raw.c_str(),msg.d_raw.size());
#if 0
if(msg.getClass() == 0x02 && msg.getType() == 0x15) { // RAWX
cerr<<"Got "<<(int)payload[11] <<" measurements "<<endl;
double rcvTow;
memcpy(&rcvTow, &payload[0], 8);
for(int n=0 ; n < payload[11]; ++n) {
else if(msg.getClass() == 0x01 && msg.getType() == 0x01) { // POSECF
// must conver this into protobuf
struct pos
{
uint32_t iTOW;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
};
pos p;
memcpy(&p, payload.c_str(), sizeof(pos));
cerr<<"Position: ("<< p.ecefX / 100000.0<<", "
<< p.ecefY / 100000.0<<", "
<< p.ecefZ / 100000.0<<") +- "<<p.pAcc<<" cm"<<endl;
double prMes;
double cpMes;
float doppler;
NavMonMessage nmm;
nmm.set_type(NavMonMessage::ObserverPositionType);
nmm.set_localutcseconds(g_gstutc.tv_sec);
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
memcpy(&prMes, &payload[16+32*n], 8);
memcpy(&cpMes, &payload[24+32*n], 8);
memcpy(&doppler, &payload[32+32*n], 4);
int gnssid = payload[36+32*n];
int sv = payload[37+32*n];
orbitcsv << std::fixed << rcvTow <<" " <<gnssid <<" " <<sv<<" "<<prMes<<" "<<cpMes <<" " << doppler<<endl;
nmm.mutable_op()->set_x(p.ecefX /100.0);
nmm.mutable_op()->set_y(p.ecefY /100.0);
nmm.mutable_op()->set_z(p.ecefZ /100.0);
nmm.mutable_op()->set_accCM(p.pAcc /100.0);
emitNMM(1, nmm);
}
}
#endif
else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX
pair<int,int> id = make_pair(payload[0], payload[1]);
auto inav = getInavFromSFRBXMsg(payload);
unsigned int wtype = getbitu(&inav[0], 0, 6);
tv[id] = timestamp;
cerr<<"gnssid "<<id.first<<" sv "<<id.second<<" " << wtype << endl;
uint32_t satTOW;
int msgTOW{0};
if(getTOWFromInav(inav, &satTOW, &g_wn)) {
cerr<<" "<<wtype<<" sv "<<id.second<<" tow "<<satTOW << " % 30 = "<< satTOW % 30<<", implied start of cycle: "<<(satTOW - (satTOW %30)) <<endl;
if(curCycleTOW != satTOW - (satTOW %30))
curCycleTOWFresh=false;
curCycleTOW = satTOW - (satTOW %30);
nextCycleTOW = curCycleTOW + 30;
}
else {
cerr<<" "<<wtype<<" sv "<<id.second<<" tow ";
if(wtype == 2) {
cerr<<"infered to be 1 "<<nextCycleTOW + 1<<endl;
curCycleTOWFresh=false;
msgTOW = nextCycleTOW + 1;
}
else if(wtype == 4) {
cerr<<"infered to be 3 "<<nextCycleTOW + 3<<endl;
msgTOW = nextCycleTOW + 3;
curCycleTOWFresh=false;
} // next have '6' which sets TOW
else if(wtype==7 || wtype == 9) {
if(curCycleTOWFresh) {
cerr<<"infered to be 7 "<< curCycleTOW + 7<<endl;
msgTOW = curCycleTOW + 7;
}
else {
cerr<<"infered to be 7 "<< nextCycleTOW + 7<<endl;
msgTOW = nextCycleTOW + 7;
}
}
else if(wtype==8 || wtype == 10) {
if(curCycleTOWFresh) {
cerr<<"infered to be 9 "<< curCycleTOW + 9<<endl;
msgTOW = curCycleTOW + 9;
}
else {
cerr<<"infered to be 9 "<< nextCycleTOW + 9<<endl;
msgTOW = nextCycleTOW + 9;
}
}
else if(wtype==1) {
if(curCycleTOWFresh) {
cerr<<"infered to be 21 "<< curCycleTOW + 21<<endl;
msgTOW = curCycleTOW + 21;
}
else {
cerr<<"infered to be 21 "<< nextCycleTOW + 21<<endl;
msgTOW = nextCycleTOW + 21;
}
}
else if(wtype==3) {
if(curCycleTOWFresh) {
msgTOW = curCycleTOW + 23;
cerr<<"infered to be 23 "<< curCycleTOW + 23 <<endl;
}
else {
cerr<<"infered to be 23 "<< nextCycleTOW + 23 <<endl;
msgTOW = nextCycleTOW + 23;
}
}
else { // dummy
cerr<<"what kind of wtype is this"<<endl;
continue;
}
NavMonMessage nmm;
nmm.set_sourceid(1);
nmm.set_type(NavMonMessage::GalileoInavType);
nmm.set_localutcseconds(g_gstutc.tv_sec);
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
nmm.mutable_gi()->set_gnsswn(g_wn);
nmm.mutable_gi()->set_gnsstow(msgTOW);
nmm.mutable_gi()->set_gnssid(id.first);
nmm.mutable_gi()->set_gnsssv(id.second);
nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size());
emitNMM(1, nmm);
}
if(0 && lasttv.count(id)) {
fmt::fprintf(stderr, "gnssid %d sv %d wtype %d, %d:%d -> %d:%d, delta=%d\n",
payload[0], payload[1], wtype, lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec);
}
lasttv[id]=tv[id];
}
else if(msg.getClass() == 1 && msg.getType() == 0x35) { // UBX-NAV-SAT
cerr<< "Info for "<<(int) payload[5]<<" svs: \n";
for(unsigned int n = 0 ; n < payload[5]; ++n) {
cerr << " "<<(payload[8+12*n] ? 'E' : 'G') << (int)payload[9+12*n] <<" db=";
cerr << (int)payload[10+12*n]<<" elev="<<(int)(char)payload[11+12*n]<<" azi=";
cerr << ((int)payload[13+12*n]*256 + payload[12+12*n])<<" prres="<< *((int16_t*)(payload.c_str()+ 14 +12*n)) *0.1 << " signal="<< ((int)(payload[16+12*n])&7) << " used="<< (payload[16+12*n]&8);
fmt::fprintf(stderr, " | %02x %02x %02x %02x\n", (int)payload[16+12*n], (int)payload[17+12*n],
(int)payload[18+12*n], (int)payload[19+12*n]);
if(payload[8+12*n]==2) { // galileo
int sv = payload[9+12*n];
auto el = (int)(char)payload[11+12*n];
auto azi = ((int)payload[13+12*n]*256 + payload[12+12*n]);
auto db = (int)payload[10+12*n];
NavMonMessage nmm;
nmm.set_sourceid(1);
nmm.set_localutcseconds(g_gstutc.tv_sec);
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
nmm.set_type(NavMonMessage::ReceptionDataType);
nmm.mutable_rd()->set_gnssid(payload[8+12*n]);
nmm.mutable_rd()->set_gnsssv(sv);
nmm.mutable_rd()->set_db(db);
nmm.mutable_rd()->set_el(el);
nmm.mutable_rd()->set_azi(azi);
nmm.mutable_rd()->set_prres(*((int16_t*)(payload.c_str()+ 14 +12*n)) *0.1);
emitNMM(1, nmm);
}
}
}
// writen2(1, payload.d_raw.c_str(),msg.d_raw.size());
}
catch(UBXMessage::BadChecksum &e) {
cerr<<"Bad UBX checksum, skipping message"<<endl;
}
}
tcsetattr(fd,TCSANOW,&oldtio);
if(!fromFile)
tcsetattr(fd,TCSANOW,&oldtio);
}