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

yolo more

pull/1/head
bert hubert 2019-08-07 15:39:45 +02:00
parent f3653f178e
commit 669f0a6a7f
4 changed files with 204 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,5 +1,6 @@
# Prerequisites
*.d
*~
# Compiled Object files
*.slo

9
ubx.cc
View File

@ -46,10 +46,15 @@ std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, st
for(unsigned int n= 0 ; n < str.size(); ++n)
msg.append(1, str[n]);
uint16_t csum = calcUbxChecksum(0x06, 0x01, msg.substr(6));
uint16_t csum = calcUbxChecksum(ubxClass, ubxType, msg.substr(6));
msg.append(1, csum&0xff);
msg.append(1, csum>>8);
/*
for(const auto& c : msg) {
fmt::fprintf(stderr, "%02x ", (int)c);
}
fmt::fprintf(stderr,"\n");
*/
return msg;
}

23
ubxparse.cc
View File

@ -704,7 +704,7 @@ Z : 5004.189 km
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);
@ -930,7 +930,7 @@ Z : 5004.189 km
double hours = ((ent.second.liveIOD().t0e - ent.second.prevIOD.second.t0e)/60.0);
double disco = sqrt((x-oldx)*(x-oldx) + (y-oldy)*(y-oldy) + (z-oldz)*(z-oldz));
cout<<ent.first<<" discontinuity "<< hours<<"h old: "<< disco <<endl;
cout<<ent.first<<" discontinuity after "<< hours<<" hours: "<< disco <<endl;
idb.addValue(sv, "iod-actual", ent.second.getIOD());
idb.addValue(sv, "iod-hours", hours);
@ -974,12 +974,29 @@ Z : 5004.189 km
else if (ubxClass == 4) { // info
fmt::printf("Log level %d: %.*s\n", (int)ubxType, msg.size(), (char*)msg.c_str());
}
else if (ubxClass == 6 && ubxType == 0) { // port config
fmt::printf("Port config: ");
for(const auto& c : msg)
fmt::printf("0x%02x,", (int)c);
cout<<"\n";
uint8_t outmask=msg[14];
if(outmask & 1)
cout<<" out UBX"<<endl;
if(outmask & 2)
cout<<" out NMEA"<<endl;
if(outmask & 32)
cout<<" out RTCM3"<<endl;
msg[14]=1;
for(const auto& c : msg)
fmt::printf("0x%02x,", (int)c);
cout<<"\n";
}
else if (ubxClass == 6 && ubxType == 1) { // msg rate
fmt::printf("Rate for class %02x type %02x: %d %d %d %d %d %d\n",
(int)msg[0], (int)msg[1], (int) msg[2], (int) msg[3],(int) msg[4],
(int) msg[5],(int) msg[6],(int) msg[7]);
}
else
cout << "ubxClass: "<<(unsigned int)ubxClass<<", ubxType: "<<(unsigned int)ubxType<<", size="<<msg.size()<<endl;
}

215
ubxtool.cc
View File

@ -1,4 +1,6 @@
#include <sys/types.h>
#include <sys/time.h>
#include <map>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
@ -11,6 +13,8 @@
#include "ubx.hh"
#include <iostream>
#include <string.h>
#include "fmt/format.h"
#include "fmt/printf.h"
using namespace std;
#define BAUDRATE B921600
@ -41,22 +45,6 @@ size_t readn2(int fd, void* buffer, size_t len)
static uint8_t getUint8(int fd)
{
uint8_t c;
if(readn2(fd, &c, 1) != sizeof(c))
throw EofException();
return c;
}
static uint16_t getUint16(int fd)
{
uint8_t c[2];
if(readn2(fd, &c, sizeof(c)) != sizeof(c))
throw EofException();
return c[0] + 256*c[1];
}
size_t writen2(int fd, const void *buf, size_t count)
{
@ -79,10 +67,107 @@ size_t writen2(int fd, const void *buf, size_t count)
}
class UBXMessage
{
public:
explicit UBXMessage(basic_string_view<uint8_t> src)
{
d_raw = src;
if(d_raw.size() < 6)
throw std::runtime_error("Partial UBX message");
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");
}
uint8_t getClass() const
{
return d_raw.at(2);
}
uint8_t getType() const
{
return d_raw.at(3);
}
std::basic_string<uint8_t> getPayload() const
{
return d_raw.substr(6, d_raw.size()-8);
}
std::basic_string<uint8_t> d_raw;
};
std::pair<struct timeval, UBXMessage> getUBXMessage(int fd)
{
uint8_t marker[2]={0};
for(;;) {
marker[0] = marker[1];
int res = readn2(fd, marker+1, 1);
if(res < 0)
throw EofException();
// cerr<<"marker now: "<< (int)marker[0]<<" " <<(int)marker[1]<<endl;
if(marker[0]==0xb5 && marker[1]==0x62) { // bingo
struct timeval tv;
gettimeofday(&tv, 0);
basic_string<uint8_t> msg;
msg.append(marker, 2); // 0,1
uint8_t b[4];
readn2(fd, b, 4);
msg.append(b, 4); // class, type, len1, len2
uint16_t len = b[2] + 256*b[3];
// cerr<<"Got class "<<(int)msg[2]<<" type "<<(int)msg[3]<<", len = "<<len<<endl;
uint8_t buffer[len+2];
res=readn2(fd, buffer, len+2);
msg.append(buffer, len+2); // checksum
return make_pair(tv, UBXMessage(msg));
}
}
}
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);
// cerr<<"Got: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
if(msg.getClass() == ubxClass && msg.getType() == ubxType) {
return msg;
}
}
throw std::runtime_error("Did not get response on time");
}
bool waitForUBXAckNack(int fd, int seconds)
{
for(int n=0; n < seconds*4; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
// cerr<<"Got: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
if(msg.getClass() == 0x05 && msg.getType() == 0x01) {
return true;
}
else if(msg.getClass() == 0x05 && msg.getType() == 0x00) {
return false;
}
}
throw std::runtime_error("Did not get ACK/NACK response on time");
}
void enableUBXMessageUSB(int fd, uint8_t ubxClass, uint8_t ubxType)
{
auto msg = buildUbxMessage(0x06, 0x01, {ubxClass, ubxType, 0, 0, 0, 1, 0, 0});
writen2(fd, msg.c_str(), msg.size());
if(waitForUBXAckNack(fd, 2))
return;
else
throw std::runtime_error("Got NACK enabling UBX message "+to_string((int)ubxClass)+" "+to_string((int)ubxType));
}
int main(int argc, char** argv)
{
int fd, res;
int fd;
struct termios oldtio,newtio;
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
@ -104,45 +189,97 @@ int main(int argc, char** argv)
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
for(int n=0; n < 10; ++n) {
auto [timestamp, msg] = getUBXMessage(fd);
cerr<<"Read some init: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl;
}
usleep(500000);
std::basic_string<uint8_t> msg = buildUbxMessage(0x06, 0x01, {0x02, 89});
std::basic_string<uint8_t> msg;
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);
cerr<<"Enabling UBX-NAV-POSECEF"<<endl;
enableUBXMessageUSB(fd, 0x01, 0x01);
/* 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 */
uint8_t marker[2]={0};
std::map<pair<int,int>, struct timeval> lasttv, tv;
for(;;) {
marker[0] = marker[1];
res = readn2(fd, marker+1, 1);
if(res < 0)
break;
// cerr<<"marker now: "<< (int)marker[0]<<" " <<(int)marker[1]<<endl;
if(marker[0]==0xb5 && marker[1]==0x62) { // bingo
basic_string<uint8_t> msg;
msg.append(marker, 2);
msg.append(1, getUint8(fd)); // class
msg.append(1, getUint8(fd)); // type
uint16_t len = getUint16(fd);
auto [timestamp, msg] = getUBXMessage(fd);
auto payload = msg.getPayload();
msg.append((uint8_t*)&len, 2);
cerr<<"Got class "<<(int)msg[0]<<" type "<<(int)msg[1]<<", len = "<<len<<endl;
uint8_t buffer[len+2];
res=readn2(fd, buffer, len+2);
msg.append(buffer, len+2); // checksum
writen2(1, msg.c_str(),msg.size());
marker[0]=marker[1]=0;
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);
}
lasttv[id]=tv[id];
}
writen2(1, msg.d_raw.c_str(),msg.d_raw.size());
}
tcsetattr(fd,TCSANOW,&oldtio);
}