yolo
bert hubert
parent
be82279efd
commit
e793f440cd
4
Makefile
4
Makefile
|
|
@ -1,6 +1,6 @@
|
|||
CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/fmt-5.2.1/include/
|
||||
|
||||
PROGRAMS = ubxparse
|
||||
PROGRAMS = ubxparse minread
|
||||
|
||||
all: $(PROGRAMS)
|
||||
|
||||
|
|
@ -10,5 +10,5 @@ clean:
|
|||
rm -f *~ *.o *.d ext/*/*.o $(PROGRAMS)
|
||||
|
||||
ubxparse: ubxparse.o ext/fmt-5.2.1/src/format.o
|
||||
g++ -std=gnu++17 $^ -o $@ -pthread
|
||||
g++ -std=gnu++17 $^ -o $@ -pthread -lncurses
|
||||
|
||||
|
|
|
|||
|
|
@ -41,11 +41,12 @@ int main()
|
|||
tcsetattr(fd,TCSANOW,&newtio);
|
||||
|
||||
for(;;) {
|
||||
res = read(fd, buf, 255);
|
||||
res = read(fd, buf, 25);
|
||||
if(res < 0)
|
||||
break;
|
||||
for(int n=0; n < res; ++n)
|
||||
printf("%c", buf[n]);
|
||||
fflush(stdout);
|
||||
}
|
||||
tcsetattr(fd,TCSANOW,&oldtio);
|
||||
}
|
||||
|
|
|
|||
305
ubxparse.cc
305
ubxparse.cc
|
|
@ -5,7 +5,10 @@
|
|||
#include "fmt/format.h"
|
||||
#include "fmt/printf.h"
|
||||
#include <fstream>
|
||||
|
||||
#include <map>
|
||||
#include <bitset>
|
||||
#include <curses.h>
|
||||
#include <vector>
|
||||
using namespace std;
|
||||
|
||||
struct EofException{};
|
||||
|
|
@ -38,6 +41,13 @@ unsigned int getbitu(const unsigned char *buff, int pos, int len)
|
|||
return bits;
|
||||
}
|
||||
|
||||
extern int getbits(const unsigned char *buff, int pos, int len)
|
||||
{
|
||||
unsigned int bits=getbitu(buff,pos,len);
|
||||
if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits;
|
||||
return (int)(bits|(~0u<<len)); /* extend sign */
|
||||
}
|
||||
|
||||
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
|
||||
{
|
||||
|
|
@ -58,18 +68,205 @@ uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_vi
|
|||
return (CK_B << 8) + CK_A;
|
||||
}
|
||||
|
||||
struct SVIOD
|
||||
{
|
||||
std::bitset<32> words;
|
||||
uint32_t t0e;
|
||||
uint8_t sisa;
|
||||
bool complete() const
|
||||
{
|
||||
return words[1] && words[3];
|
||||
}
|
||||
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 = 60*getbitu(&page[0], 16, 14);
|
||||
}
|
||||
else if(wtype == 3) {
|
||||
sisa = getbitu(&page[0], 120, 8);
|
||||
}
|
||||
}
|
||||
|
||||
struct SVStat
|
||||
{
|
||||
uint8_t e5bhs{0}, e1bhs{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"
|
||||
uint32_t a0{0}, a1{0};
|
||||
map<int, SVIOD> iods;
|
||||
void addWord(std::basic_string_view<uint8_t> page);
|
||||
bool completeIOD() const;
|
||||
uint16_t getIOD() const;
|
||||
SVIOD liveIOD() const;
|
||||
};
|
||||
|
||||
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 >=1 && wtype <= 4) { // ephemeris
|
||||
uint16_t iod = getbitu(&page[0], 6, 10);
|
||||
iods[iod].addWord(page);
|
||||
if(iods[iod].complete()) {
|
||||
SVIOD latest = iods[iod];
|
||||
iods.clear();
|
||||
iods[iod] = latest;
|
||||
}
|
||||
}
|
||||
else if(wtype==5) { // disturbance, health, time
|
||||
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);
|
||||
// cout<<"Setting tow to "<<tow<<endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
struct WinKeeper
|
||||
{
|
||||
WinKeeper();
|
||||
struct Window
|
||||
{
|
||||
WINDOW *header, *text;
|
||||
int sv;
|
||||
void setHeader(std::string_view line)
|
||||
{
|
||||
wclear(header);
|
||||
wmove(header, 0, 0);
|
||||
wattron(header, A_BOLD | A_UNDERLINE);
|
||||
wprintw(header, "%s", &line[0]);
|
||||
wattroff(header, A_BOLD | A_UNDERLINE);
|
||||
wrefresh(header);
|
||||
}
|
||||
void setStatus(std::string_view line)
|
||||
{
|
||||
wmove(header, 1, 0);
|
||||
wattron(header, A_BOLD);
|
||||
wprintw(header, "%s", &line[0]);
|
||||
wattroff(header, A_BOLD);
|
||||
wrefresh(header);
|
||||
}
|
||||
|
||||
void emitLine(std::string_view line)
|
||||
{
|
||||
wprintw(text, "%s\n", &line[0]);
|
||||
wrefresh(text);
|
||||
}
|
||||
};
|
||||
vector<Window> d_windows;
|
||||
static int d_h, d_w;
|
||||
void emitLine(int sv, std::string_view line);
|
||||
void setStatus(int sv, std::string_view line);
|
||||
};
|
||||
|
||||
int WinKeeper::d_h;
|
||||
int WinKeeper::d_w;
|
||||
|
||||
WinKeeper::WinKeeper()
|
||||
{
|
||||
initscr();
|
||||
getmaxyx(stdscr, d_h, d_w);
|
||||
|
||||
int winwidth = d_w / 6;
|
||||
for(int n=0; n < 5 ; ++n) {
|
||||
d_windows.push_back({
|
||||
newwin(3, winwidth, 0, n*(winwidth+2)),
|
||||
newwin(d_h-3, winwidth, 3, n*(winwidth+2)),
|
||||
0});
|
||||
scrollok(d_windows[n].text, 1);
|
||||
}
|
||||
};
|
||||
|
||||
void WinKeeper::emitLine(int sv, std::string_view line)
|
||||
{
|
||||
for(auto& w: d_windows) {
|
||||
if(w.sv == sv) {
|
||||
w.emitLine(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// nothing matched
|
||||
for(auto& w: d_windows) {
|
||||
if(!w.sv) {
|
||||
w.sv = sv;
|
||||
w.setHeader(std::to_string(sv));
|
||||
w.emitLine(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv));
|
||||
}
|
||||
|
||||
void WinKeeper::setStatus(int sv, std::string_view line)
|
||||
{
|
||||
for(auto& w: d_windows) {
|
||||
if(w.sv == sv) {
|
||||
w.setStatus(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// nothing matched
|
||||
for(auto& w: d_windows) {
|
||||
if(!w.sv) {
|
||||
w.sv = sv;
|
||||
w.setHeader(std::to_string(sv));
|
||||
w.setStatus(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv));
|
||||
}
|
||||
|
||||
|
||||
std::map<int, SVStat> g_svstats;
|
||||
|
||||
int main()
|
||||
try
|
||||
{
|
||||
unsigned int tow=0, wn=0;
|
||||
WinKeeper wk;
|
||||
|
||||
// unsigned int tow=0, wn=0;
|
||||
ofstream csv("iod.csv");
|
||||
ofstream csv2("toe.csv");
|
||||
csv<<"timestamp sv iod sisa"<<endl;
|
||||
csv2<<"timestamp sv tow toe"<<endl;
|
||||
int tow=0;
|
||||
for(;;) {
|
||||
auto c = getUint8();
|
||||
if(c != 0xb5) {
|
||||
cout << (char)c;
|
||||
// cout << (char)c;
|
||||
continue;
|
||||
}
|
||||
c = getUint8();
|
||||
|
|
@ -84,7 +281,7 @@ try
|
|||
uint8_t ubxType = getUint8();
|
||||
uint16_t msgLen = getUint16();
|
||||
|
||||
cout <<"Had an ubx message of class "<<(int) ubxClass <<" and type "<< (int) ubxType << " of " << msgLen <<" bytes"<<endl;
|
||||
// cout <<"Had an ubx message of class "<<(int) ubxClass <<" and type "<< (int) ubxType << " of " << msgLen <<" bytes"<<endl;
|
||||
|
||||
std::basic_string<uint8_t> msg;
|
||||
msg.reserve(msgLen);
|
||||
|
|
@ -94,42 +291,49 @@ try
|
|||
uint16_t ubxChecksum = getUint16();
|
||||
if(ubxChecksum != calcUbxChecksum(ubxClass, ubxType, msg)) {
|
||||
cout<<"Checksum: "<<ubxChecksum<< ", calculated: "<<
|
||||
calcUbxChecksum(ubxClass, ubxType, msg)<<endl;
|
||||
calcUbxChecksum(ubxClass, ubxType, msg)<<"\n";
|
||||
}
|
||||
|
||||
if(ubxClass == 2 && ubxType == 89) { // SAR
|
||||
cout<<"SAR: sv = "<< (int)msg[2] <<" ";
|
||||
for(int n=4; n < 12; ++n)
|
||||
cout << (int)msg[n]<<" ";
|
||||
cout << "Type: "<< msg[12] <<endl;
|
||||
cout<<endl;
|
||||
// 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";
|
||||
}
|
||||
if(ubxClass == 2 && ubxType == 19) { //UBX-RXM-SFRBX
|
||||
cout<<"SFRBX GNSSID "<< (int)msg[0]<<", SV "<< (int)msg[1];
|
||||
cout<<" words "<< (int)msg[4]<<", version "<< (int)msg[6];
|
||||
cout<<endl;
|
||||
if(msg[0] != 2)
|
||||
// cout<<"SFRBX GNSSID "<< (int)msg[0]<<", SV "<< (int)msg[1];
|
||||
// cout<<" words "<< (int)msg[4]<<", version "<< (int)msg[6];
|
||||
// cout<<"\n";
|
||||
if(msg[0] != 2) // version field
|
||||
continue;
|
||||
// 7 is reserved
|
||||
|
||||
|
||||
// cout << ((msg[8]&128) ? "Even " : "Odd ");
|
||||
// cout << ((msg[8]&64) ? "Alert " : "Nominal ");
|
||||
unsigned int wtype = (int)(msg[11] & (~(64+128)));
|
||||
unsigned int sv = (int)msg[1];
|
||||
cout << "Word type "<< (int)(msg[11] & (~(64+128))) <<" SV " << (int)msg[1]<<endl;
|
||||
for(unsigned int n = 8; n < msg.size() ; ++n) {
|
||||
fmt::printf("%02x ", msg[n]);
|
||||
}
|
||||
cout<<"\n";
|
||||
// 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]);
|
||||
|
||||
for(auto& c : payload)
|
||||
fmt::printf("%02x ", c);
|
||||
// for(auto& c : payload)
|
||||
// fmt::printf("%02x ", c);
|
||||
|
||||
cout<<endl;
|
||||
// cout<<"\n";
|
||||
|
||||
std::basic_string<uint8_t> inav;
|
||||
unsigned int i,j;
|
||||
|
|
@ -138,35 +342,42 @@ try
|
|||
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);
|
||||
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
|
||||
// for(auto& c : inav)
|
||||
// fmt::printf("%02x ", c);
|
||||
|
||||
cout<<endl;
|
||||
if(wtype == 1 && tow) {
|
||||
time_t t = 935280000 + wn * 7*86400 + tow;
|
||||
csv2<<t<<" "<<sv<<" " << tow<<" "<<60*getbitu(inav.c_str(), 16, 14)<<"\n";
|
||||
g_svstats[sv].addWord(inav);
|
||||
if(wtype >=1 && wtype <= 4) { // ephemeris
|
||||
uint16_t iod = getbitu(&inav[0], 6, 10);
|
||||
if(wtype == 1 && tow) {
|
||||
int t0e = 60*getbitu(&inav[0], 16, 14);
|
||||
int age = (tow - t0e)/60;
|
||||
wk.emitLine(sv, "Eph" +std::to_string(wtype)+", iod=" + to_string(iod)+", age="+to_string(age)+"m" );
|
||||
}
|
||||
else
|
||||
wk.emitLine(sv, "Eph" +std::to_string(wtype)+", iod=" + to_string(iod));
|
||||
}
|
||||
if(wtype == 3 && tow) {
|
||||
time_t t = 935280000 + wn * 7*86400 + tow;
|
||||
csv<<t<<" "<<sv<<" " << getbitu(inav.c_str(), 6, 10)<<" " << getbitu(inav.c_str(), 120, 8) << endl;
|
||||
else
|
||||
wk.emitLine(sv, "Word "+std::to_string(wtype));
|
||||
if(wtype == 5) {
|
||||
wk.setStatus(sv, "Health: "+std::to_string(getbitu(&inav[0], 67, 2)));
|
||||
tow = getbitu(&inav[0], 85, 20);
|
||||
}
|
||||
if(wtype==5) {
|
||||
tow = getbitu(inav.c_str(), 85, 20);
|
||||
wn = getbitu(inav.c_str(), 73, 12);
|
||||
cout<<"gst wn " << wn << " tow "<<tow<<endl;
|
||||
unsigned int e5bhs = getbitu(inav.c_str(), 67, 2);
|
||||
unsigned int e1bhs = getbitu(inav.c_str(), 69, 2);
|
||||
unsigned int e5bdvs = getbitu(inav.c_str(), 71, 1);
|
||||
unsigned int e1bdvs = getbitu(inav.c_str(), 72, 1);
|
||||
cout<<"health: sv "<< sv<<" " << e5bhs << " " << e1bhs <<" " << e5bdvs<<" " << e1bdvs <<endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// time_t t = 935280000 + wn * 7*86400 + tow;
|
||||
/*
|
||||
for(const auto& ent : g_svstats) {
|
||||
// 12 iod t0e
|
||||
fmt::printf("%2d\t", ent.first);
|
||||
if(ent.second.completeIOD()) {
|
||||
cout << ent.second.getIOD() << "\t" << ( ent.second.tow - ent.second.liveIOD().t0e ) << "\t" << (unsigned int)ent.second.liveIOD().sisa;
|
||||
}
|
||||
cout<<"\n";
|
||||
}
|
||||
cout<<"\n";
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch(EofException& e)
|
||||
{}
|
||||
|
|
|
|||
Loading…
Reference in New Issue