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

the rest

pull/1/head
bert hubert 2019-08-10 00:54:35 +02:00
parent 0c6a715a4a
commit f86ce16591
3 changed files with 204 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
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 navparse ubxdisplay minread ubxtool
PROGRAMS = navparse ubxtool navnexus
all: $(PROGRAMS)
@ -11,19 +11,17 @@ clean:
H2OPP=ext/powerblog/h2o-pp.o
SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simplesocket/sclasses.o ext/powerblog/ext/simplesocket/comboaddress.o
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 -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf # -lwslay
navnexus: navnexus.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 -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 bits.o ext/fmt-5.2.1/src/format.o galileo.o navmon.pb.o
g++ -std=gnu++17 $^ -o $@ -lprotobuf

153
navnexus.cc 100644
View File

@ -0,0 +1,153 @@
#include "comboaddress.hh"
#include "sclasses.hh"
#include <thread>
#include <signal.h>
#include "navmon.pb.h"
#include "fmt/format.h"
#include "fmt/printf.h"
#include <mutex>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
using namespace std;
std::mutex g_clientmut;
set<int> g_clients;
std::mutex g_dedupmut;
set<std::tuple<int, int, int, int>> g_dedup;
int g_store;
std::multimap<pair<uint32_t,uint32_t>, string> g_history;
void recvSession(int s, ComboAddress client)
{
cerr<<"Receiving messages from "<<client.toStringWithPort()<<endl;
for(;;) {
string part=SRead(s, 4);
if(part != "bert") {
cerr << "Wrong magic!"<<endl;
break;
}
string out=part;
part = SRead(s, 2);
out += part;
uint16_t len;
memcpy(&len, part.c_str(), 2);
len = htons(len);
part = SRead(s, len);
out += part;
NavMonMessage nmm;
nmm.ParseFromString(part);
// cerr<<nmm.sourceid()<<" ";
if(nmm.type() == NavMonMessage::GalileoInavType) {
std::lock_guard<std::mutex> lg(g_dedupmut);
if(g_dedup.count({nmm.gi().gnssid(), nmm.gi().gnsssv(), nmm.gi().gnsswn(), nmm.gi().gnsstow()})) {
cerr<<"Dedupped message from "<< nmm.sourceid()<<" "<< fmt::format("{0} {1} {2} {3}", nmm.gi().gnssid(), nmm.gi().gnsssv(), nmm.gi().gnsswn(), nmm.gi().gnsstow()) << endl;
continue;
}
cerr<<"New message from "<< nmm.sourceid()<<" "<< fmt::format("{0} {1} {2} {3}", nmm.gi().gnssid(), nmm.gi().gnsssv(), nmm.gi().gnsswn(), nmm.gi().gnsstow()) << endl;
g_dedup.insert({nmm.gi().gnssid(), nmm.gi().gnsssv(), nmm.gi().gnsswn(), nmm.gi().gnsstow()});
}
else
; // cerr<<"Not an inav message "<< (int) nmm.type()<<endl;
g_history.insert({{nmm.localutcseconds(), nmm.localutcnanoseconds()}, out});
for(const auto& fd : g_clients) {
SWrite(fd, out);
}
}
}
void recvListener(Socket&& s, ComboAddress local)
{
for(;;) {
ComboAddress remote=local;
int fd = SAccept(s, remote);
std::thread t(recvSession, fd, remote);
t.detach();
}
}
void sendSession(int s, ComboAddress client)
{
cerr<<"New downstream client "<<client.toStringWithPort() << endl;
pair<uint32_t, uint32_t> start;
start.first=time(0)-1800;
start.second=0;
int count =0;
for(auto iter = g_history.lower_bound(start); iter != g_history.end(); ++iter) {
SWriten(s, iter->second);
++count;
}
cerr<<"Wrote "<<count<<" historical messages"<<endl;
g_clients.insert(s);
char c;
int res = read(s, &c, 1);
g_clients.erase(s);
close(s);
cerr<<"Disconnect "<<client.toStringWithPort() << endl;
}
void sendListener(Socket&& s, ComboAddress local)
{
for(;;) {
ComboAddress remote=local;
int fd = SAccept(s, remote);
std::thread t(sendSession, fd, remote);
t.detach();
}
}
int main(int argc, char** argv)
{
signal(SIGPIPE, SIG_IGN);
g_store = open("permanent", O_CREAT | O_APPEND | O_WRONLY, 0666);
if(g_store < 0) {
cerr<<"Unable to open permanent storage file"<<endl;
return(EXIT_FAILURE);
}
ComboAddress recvaddr("0.0.0.0", 29600);
Socket receiver(recvaddr.sin4.sin_family, SOCK_STREAM, 0);
SSetsockopt(receiver,SOL_SOCKET, SO_REUSEADDR, 1 );
SBind(receiver, recvaddr);
SListen(receiver, 128);
thread recvThread(recvListener, std::move(receiver), recvaddr);
recvThread.detach();
ComboAddress sendaddr("0.0.0.0", 29601);
Socket sender(sendaddr.sin4.sin_family, SOCK_STREAM);
SSetsockopt(sender, SOL_SOCKET, SO_REUSEADDR, 1 );
SBind(sender, sendaddr);
SListen(sender, 128);
thread sendThread(sendListener, std::move(sender), sendaddr);
sendThread.detach();
for(;;) {
sleep(1);
}
}

71
navparse.cc
View File

@ -24,6 +24,12 @@ struct EofException{};
Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000);
struct GNSSReceiver
{
Point position;
};
int g_dtLS{18};
@ -108,6 +114,14 @@ void SVIOD::addWord(std::basic_string_view<uint8_t> page)
}
struct SVPerRecv
{
int el{-1}, azi{-1}, db{-1};
uint16_t wn;
uint32_t tow; // last seen
};
struct SVStat
{
uint8_t e5bhs{0}, e1bhs{0};
@ -124,7 +138,9 @@ struct SVStat
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<uint64_t, SVPerRecv> perrecv;
map<int, SVIOD> iods;
void addWord(std::basic_string_view<uint8_t> page);
bool completeIOD() const;
@ -535,6 +551,8 @@ try
map<int, int> utcstats, gpsgststats;
for(const auto& s: g_svstats) {
if(!s.second.wn) // this will suck in 20 years
continue;
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;
@ -551,7 +569,8 @@ try
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);
// cout<<"sv: "<<sv<<", shift: "<<shift<<" a0: "<< g_svstats[sv].a0 << ", a1: "<< g_svstats[sv].a1 <<", age: "<< utcstats.begin()->first<<endl;
ret["utc-offset-ns"] = 1.073741824*ldexp(1.0*shift, -20);
ret["leap-second-planned"] = (g_svstats[sv].dtLSF != g_svstats[sv].dtLS);
}
@ -561,7 +580,7 @@ try
}
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
long shift = g_svstats[sv].a0g * (1L<<16) + g_svstats[sv].a1g * gpsgststats.begin()->first; // in 2^-51 seconds units
ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21);
}
@ -587,8 +606,16 @@ try
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;
nlohmann::json perrecv = nlohmann::json::object();
for(const auto& pr : s.second.perrecv) {
nlohmann::json det = nlohmann::json::object();
det["elev"] = pr.second.el;
det["db"] = pr.second.db;
det["last-seen-s"] = (7*86400*(latestWN() - pr.second.wn) + latestTow() - (int)pr.second.tow);
perrecv[to_string(pr.first)]=det;
}
item["perrecv"]=perrecv;
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;
@ -606,20 +633,13 @@ try
Vector core2us(core, our);
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
// https://ds9a.nl/articles/
/* 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;
item["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;
@ -682,19 +702,22 @@ try
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();
g_svstats[sv].perrecv[nmm.sourceid()].db = nmm.rd().db();
g_svstats[sv].perrecv[nmm.sourceid()].el = nmm.rd().el();
g_svstats[sv].perrecv[nmm.sourceid()].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);
// 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();
g_svstats[sv].perrecv[nmm.sourceid()].wn = nmm.gi().gnsswn();
g_svstats[sv].perrecv[nmm.sourceid()].tow = nmm.gi().gnsstow();
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
// for(auto& c : inav)
@ -838,9 +861,9 @@ try
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";
// 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);