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

various improvements

pull/1/head
bert hubert 2019-08-29 12:45:37 +02:00
parent 3e6ab0639b
commit bca8a0b2b2
9 changed files with 6258 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Makefile
View File

@ -1,6 +1,8 @@
CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/CLI11 -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ -I/usr/local/opt/openssl/include/ -Wno-delete-non-virtual-dtor
CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/CLI11 -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ -I/usr/local/opt/openssl/include/
PROGRAMS = navparse ubxtool navnexus navrecv navdump
# CXXFLAGS += -Wno-delete-non-virtual-dtor
PROGRAMS = navparse ubxtool navnexus navrecv navdump testrunner
all: navmon.pb.cc $(PROGRAMS)
@ -32,3 +34,6 @@ navmon.pb.cc: navmon.proto
ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o
$(CXX) -std=gnu++17 $^ -o $@ -lprotobuf
testrunner: navmon.pb.o testrunner.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o ephemeris.o
$(CXX) -std=gnu++17 $^ -o $@ -lprotobuf

5939
ext/doctest.h 100644
View File

File diff suppressed because it is too large Load Diff

229
galileo.hh
View File

@ -1,5 +1,234 @@
#pragma once
#include <stdint.h>
#include <string>
#include <vector>
#include <functional>
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn);
struct GalileoMessage
{
uint8_t wtype;
typedef void (GalileoMessage::*func_t)(std::basic_string_view<uint8_t> page);
std::vector<func_t> parsers{
&GalileoMessage::parse0,
&GalileoMessage::parse1,
&GalileoMessage::parse2,
&GalileoMessage::parse3,
&GalileoMessage::parse4,
&GalileoMessage::parse5,
&GalileoMessage::parse6,
&GalileoMessage::parse7,
&GalileoMessage::parse8,
&GalileoMessage::parse9,
&GalileoMessage::parse10
};
int parse(std::basic_string_view<uint8_t> page)
{
wtype = getbitu(&page[0], 0, 6);
std::invoke(parsers.at(wtype), this, page);
return wtype;
}
uint8_t sparetime{0};
uint16_t wn{0}; // we put the "unrolled" week number here!
uint32_t tow{0}; // "last seen"
int iodalmanac;
int wnalmanac;
int t0almanac;
int svid1, svid2, svid3;
// spare word, only contains a WN and a TOW, but only if the 'time' field is set to 2
void parse0(std::basic_string_view<uint8_t> page)
{
sparetime = getbitu(&page[0], 6, 2);
if(sparetime == 2) {
wn = getbitu(&page[0], 96, 12);
tow = getbitu(&page[0], 108, 20);
}
}
GalileoMessage()
{
}
uint8_t e5bhs{0}, e1bhs{0};
uint8_t gpshealth{0};
uint16_t ai0{0};
int16_t ai1{0}, ai2{0};
bool sf1{0}, sf2{0}, sf3{0}, sf4{0}, sf5{0};
int BGDE1E5a{0}, BGDE1E5b{0};
bool e5bdvs{false}, e1bdvs{false};
bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
//
// 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
uint32_t t0e;
uint32_t e, sqrtA;
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
// 60 seconds
uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS
// 2^-34 2^-46
int32_t af0{-1} , af1{-1};
// 2^-59
int8_t af2{-1};
uint8_t sisa;
uint16_t iodnav;
struct Almanac
{
int svid{-1};
int af0, af1;
int e1bhs, e5bhs;
} alma1, alma2, alma3;
// an ephemeris word
void parse1(std::basic_string_view<uint8_t> page)
{
iodnav = getbitu(&page[0], 6, 10);
t0e = getbitu(&page[0], 16, 14);
m0 = getbits(&page[0], 30, 32);
e = getbitu(&page[0], 62, 32);
sqrtA = getbitu(&page[0], 94, 32);
}
// another ephemeris word
void parse2(std::basic_string_view<uint8_t> page)
{
iodnav = getbitu(&page[0], 6, 10);
omega0 = getbits(&page[0], 16, 32);
i0 = getbits(&page[0], 48, 32);
omega = getbits(&page[0], 80, 32);
idot = getbits(&page[0], 112, 14);
}
// yet another ephemeris word
void parse3(std::basic_string_view<uint8_t> page)
{
iodnav = getbitu(&page[0], 6, 10);
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);
}
// can't get enough of that ephemeris
void parse4(std::basic_string_view<uint8_t> page)
{
iodnav = getbitu(&page[0], 6, 10);
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);
}
// ionospheric disturbance, health, group delay, time
void parse5(std::basic_string_view<uint8_t> page)
{
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);
}
// time stuff
void parse6(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);
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);
tow = getbitu(&page[0], 105, 20);
}
// almanac
void parse7(std::basic_string_view<uint8_t> page)
{
iodalmanac = getbitu(&page[0], 6, 4);
wnalmanac = getbitu(&page[0], 10, 2);
t0almanac = getbitu(&page[0], 12, 10);
alma1.svid = getbitu(&page[0], 22, 6);
}
// almanac
void parse8(std::basic_string_view<uint8_t> page)
{
alma1.af0 = getbits(&page[0], 10, 16);
alma1.af1 = getbits(&page[0], 26, 13);
alma1.e5bhs = getbitu(&page[0], 39, 2);
alma1.e1bhs = getbitu(&page[0], 41, 2);
alma2.svid = getbitu(&page[0], 43, 6);
}
// almanac
void parse9(std::basic_string_view<uint8_t> page)
{
alma2.af0 = getbits(&page[0], 38, 16);
alma2.af1 = getbits(&page[0], 54, 13);
alma2.e5bhs = getbitu(&page[0], 67, 2);
alma2.e1bhs = getbitu(&page[0], 69, 2);
alma3.svid = getbitu(&page[0], 71, 6);
}
// almanac + more time stuff (GPS)
void parse10(std::basic_string_view<uint8_t> page)
{
alma3.af0 = getbits(&page[0], 53, 16);
alma3.af1 = getbits(&page[0], 69, 13);
alma3.e5bhs = getbitu(&page[0], 82, 2);
alma3.e1bhs = getbitu(&page[0], 84, 2);
a0g = getbits(&page[0], 86, 16);
a1g = getbits(&page[0], 102, 12);
t0g = getbitu(&page[0], 114, 8);
wn0g = getbitu(&page[0], 122, 6);
}
};

5
minicurl.cc
View File

@ -148,8 +148,11 @@ std::string MiniCurl::postURL(const std::string& str, const std::string& postdat
long http_code = 0;
curl_easy_getinfo(d_curl, CURLINFO_RESPONSE_CODE, &http_code);
if(res != CURLE_OK)
if(res != CURLE_OK) {
cerr<<"Detailed error: "<<d_data<<endl;
cerr<<postdata<<endl;
throw std::runtime_error("Unable to post URL ("+std::to_string(http_code)+"): "+string(curl_easy_strerror(res)));
}
std::string ret=d_data;

42
navdump.cc
View File

@ -1,5 +1,6 @@
#include <stdio.h>
#include <string>
#include <stdio.h>
#include <iostream>
#include <arpa/inet.h>
#include "fmt/format.h"
@ -19,6 +20,7 @@
#include "gps.hh"
#include "glonass.hh"
#include "beidou.hh"
#include "galileo.hh"
#include <unistd.h>
using namespace std;
@ -60,8 +62,37 @@ int main(int argc, char** argv)
}
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;
static map<int, GalileoMessage> gms;
GalileoMessage& gm = gms[nmm.gi().gnsssv()];
int wtype = gm.parse(inav);
cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype << endl;
if(wtype == 4) {
// 2^-34 2^-46
cout <<" af0 "<<gm.af0 <<" af1 "<<gm.af1 <<", scaled: "<<ldexp(1.0*gm.af0, 19-34)<<", "<<ldexp(1.0*gm.af1, 38-46)<<endl;
}
static uint32_t tow;
if(wtype == 0 || wtype == 5 || wtype == 6)
tow = gm.tow;
if(wtype < 7)
gm = GalileoMessage{};
// af0 af1 scaling in almanac: 2^-19, 2^2^-38 plus "truncated"
if(wtype == 7) {
cout<<" t0a "<<gm.t0almanac<<", alma sv1 "<<gm.alma1.svid<<", t0a age: "<< ephAge(gm.t0almanac *600, tow) << endl;
}
else if(wtype == 8 && gm.alma1.svid > 0) {
cout<<" "<<gm.alma1.svid<<" af0 "<<gm.alma1.af0<<" af1 "<< gm.alma1.af1 <<" e5bhs "<< gm.alma1.e5bhs<<" e1bhs "<< gm.alma1.e1bhs<<endl;
}
else if(wtype == 9 && gm.alma2.svid > 0) {
cout<<" "<<gm.alma2.svid<<" af0 "<<gm.alma2.af0<<" af1 "<< gm.alma2.af1 <<" e5bhs "<< gm.alma2.e5bhs<<" e1bhs "<< gm.alma2.e1bhs<<endl;
}
else if(wtype == 10 && gm.alma3.svid > 0){
cout<<" "<<gm.alma3.svid<<" af0 "<<gm.alma3.af0<<" af1 "<< gm.alma3.af1 <<" e5bhs "<< gm.alma3.e5bhs<<" e1bhs "<< gm.alma3.e1bhs<<endl;
}
}
else if(nmm.type() == NavMonMessage::GPSInavType) {
int sv = nmm.gpsi().gnsssv();
@ -84,7 +115,10 @@ int main(int argc, char** argv)
BeidouMessage bm;
uint8_t pageno;
int fraid = bm.parse(cond, &pageno);
cout<<"BeiDou "<<sv<<": "<<bm.sow<<", FraID "<<fraid<<endl;
cout<<"BeiDou "<<sv<<": "<<bm.sow<<", FraID "<<fraid;
if(fraid == 1)
cout<<" wn "<<bm.wn<<" t0c "<<(int)bm.t0c<<" aodc "<< (int)bm.aodc <<" aode "<< (int)bm.aode <<" sath1 "<< (int)bm.sath1 << endl;
cout<<endl;
}
else if(nmm.type() == NavMonMessage::ObserverPositionType) {

12
navparse.cc
View File

@ -359,7 +359,15 @@ int latestTow(int gnssid)
uint64_t nanoTime(int gnssid, int wn, int tow)
{
int offset = (gnssid == 0) ? 315964800 : 935280000;
int offset;
if(gnssid == 0) // GPS
offset = 315964800;
if(gnssid == 2) // Galileo, 22-08-1999
offset = 935280000;
if(gnssid == 3) // Beidou, 01-01-2006
offset = 1136073600;
if(gnssid == 6) // GLONASS
throw std::runtime_error("GLONASS does not have WN/TOW");
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); // Leap!!
}
@ -401,6 +409,8 @@ struct InfluxPusher
{
if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0)
return;
if(id.first == 3)
cout << g_svstats[id].wn <<", "<<g_svstats[id].tow<<" -> " <<nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow)<<endl;
d_buffer+= string(name) +",gnssid="+to_string(id.first)+",sv=" +to_string(id.second) + " value="+to_string(value)+" "+
to_string(nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow))+"\n";

6
navrecv.cc
View File

@ -53,8 +53,12 @@ void recvSession(int s, ComboAddress client)
cerr<<"Receiving messages from "<<client.toStringWithPort()<<endl;
for(;;) {
string part=SRead(sock, 4);
if(part.empty()) {
cerr<<"EOF from "<<client.toStringWithPort()<<endl;
break;
}
if(part != "bert") {
cerr << "Wrong magic!"<<endl;
cerr << "Wrong magic from "<<client.toStringWithPort()<<endl;
break;
}
string out=part;

22
testrunner.cc 100644
View File

@ -0,0 +1,22 @@
#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include "ext/doctest.h"
#include "ephemeris.hh"
TEST_CASE("testing ephemeris age") {
CHECK(ephAge(0,0) == 0);
CHECK(ephAge(100,0) == 100);
CHECK(ephAge(0,100) == -100);
CHECK(ephAge(0,2*86400) == -2*86400);
CHECK(ephAge(0,3*86400) == -3*86400);
CHECK(ephAge(0, 3.49*86400) == -3.49*86400);
CHECK(ephAge(0, 3.51*86400) != -3.51*86400);
CHECK(ephAge(2*86400, 0) == 2*86400);
CHECK(ephAge(3*86400, 0) == 3*86400);
CHECK(ephAge(3.49*86400, 0) == 3.49*86400);
}

6
ubxtool.cc
View File

@ -388,7 +388,7 @@ int main(int argc, char** argv)
readSome(fd);
std::basic_string<uint8_t> msg;
if(0) {
cerr<<"Sending a soft reset"<<endl;
msg = buildUbxMessage(0x06, 0x04, {0x00, 0x00, 0x01, 0x00});
writen2(fd, msg.c_str(), msg.size());
@ -408,7 +408,7 @@ int main(int argc, char** argv)
}
break;
}
}
cerr<<"Sending GNSS query"<<endl;
msg = buildUbxMessage(0x06, 0x3e, {});
writen2(fd, msg.c_str(), msg.size());
@ -709,7 +709,7 @@ int main(int argc, char** argv)
msgTOW = curCycleTOW + 23;
}
else { // dummy
cerr<<"what kind of wtype is this"<<endl;
cerr<<"galileo E"<<id.second<<" what kind of wtype is this: "<<wtype<<endl;
continue;
}
}