128 lines
4.0 KiB
C++
128 lines
4.0 KiB
C++
#include <string>
|
|
#include "fmt/format.h"
|
|
#include "fmt/printf.h"
|
|
#include <vector>
|
|
#include <map>
|
|
#include <iostream>
|
|
#include "ubx.hh"
|
|
using namespace std;
|
|
/* TRK-MEAS
|
|
|
|
2 U1 Number of channels tracked
|
|
?
|
|
8 U4 Poweron counter milliseconds
|
|
12 U4 "session time counter" milliseconds
|
|
|
|
104 + n*56 + 0 U1 Channel (?)
|
|
104 + n*56 + 1 U1 Quality indicator 0 idle, 1 search, 2 acquired, 3 unsusable, 4 code lock, 5/6/7 carrier lock)
|
|
+ 2 ?
|
|
+ 3 ?
|
|
104 + n*56 + 4 U1 GNSS (?)
|
|
104 + n*56 + 5 U1 Satellite ID
|
|
?
|
|
104 + n*56 + 7 U1 GLONASS Frequency
|
|
104 + n*56 + 8 U1 Tracking status
|
|
?
|
|
+ 16 U1 Code lock "count"
|
|
+ 17 U1 Phase lock "count"
|
|
?
|
|
+ 20 U2 256*dB
|
|
?
|
|
104 + n*56 + 24 I8 Transmission time, in units of 2^-32 milliseconds (?) // ts=I8(p+24)*P2_32/1000.0;
|
|
|
|
+ 32 I8 "ADR" in units of 2^-32, plus 0.5 if 'bit' 0x40 is set in tracking status
|
|
+ 40 I4 Doppler in units of 2^-10 deca-hertz (?) // I4(p+40)*P2_10*10.0;
|
|
+ 44 ?
|
|
..
|
|
+ 56
|
|
*/
|
|
|
|
extern "C" {
|
|
int ubxdecrypt(const unsigned char crypto[16], unsigned char plaintext[16]) __attribute__((weak));
|
|
int ubxdecrypt(const unsigned char crypto[16], unsigned char plaintext[16])
|
|
{
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
vector<TrkSatStat> parseTrkMeas(std::basic_string_view<uint8_t> payload)
|
|
{
|
|
uint8_t plainchunk[16];
|
|
std::basic_string<uint8_t> plaintext;
|
|
/*
|
|
cerr<<"payload.size(): "<<payload.size()<<", mod "<<(payload.size()-4)%16<<", " << payload[2]+256*payload[3]<<": ";
|
|
for(int n=0; n < 4; ++n)
|
|
cerr<<fmt::sprintf("%02x ", (unsigned int) payload[n]);
|
|
cerr<<endl;
|
|
*/
|
|
|
|
vector<TrkSatStat> ret;
|
|
for(unsigned int n=4 ; n < payload.size(); n+=16) {
|
|
if(ubxdecrypt(&payload[n], plainchunk) < 0)
|
|
return ret;
|
|
plaintext.append(plainchunk, plainchunk+16);
|
|
}
|
|
/*
|
|
int msgsize = (payload[2]+256*payload[3]) - 6;
|
|
cerr<<"msgsize: "<<msgsize<<", assumed SVs: "<<(int)plaintext[8]<<", per "<<(msgsize-104.0)/(int)plaintext[8]<<endl;
|
|
*/
|
|
|
|
int64_t maxtr=0;
|
|
for(int n = 0 ; n < plaintext[8]; ++n) {
|
|
int offset = 110+n*56;
|
|
int64_t tr;
|
|
memcpy(&tr, &plaintext[offset+24], 8);
|
|
if(tr > maxtr)
|
|
maxtr = tr;
|
|
}
|
|
|
|
|
|
|
|
for(int n = 0 ; n < plaintext[8]; ++n) {
|
|
int offset = 110+n*56;
|
|
int32_t rdoppler;
|
|
int64_t tr;
|
|
memcpy(&rdoppler, &plaintext[offset+40], 4);
|
|
|
|
memcpy(&tr, &plaintext[offset+24], 8);
|
|
|
|
int gnssid = plaintext[offset+4];
|
|
int sv = plaintext[offset+5];
|
|
double doppler = ldexp(1.0*rdoppler,-10)*10;
|
|
int plcount = (unsigned int)plaintext[offset+17];
|
|
/*
|
|
int trkstat = plaintext[offset+8];
|
|
cerr<<" gnssid " << gnssid <<" sv "<< sv;
|
|
cerr<<" qi "<<(unsigned int)plaintext[offset+1] <<" c "<<(unsigned int)plaintext[offset];
|
|
cerr<<" ? "<<(unsigned int)plaintext[offset+2] << " ? " <<(unsigned int)plaintext[offset+3];
|
|
cerr<<" trkstat " << trkstat<<" db " << (unsigned int) plaintext[offset+21] << " doppler "<<
|
|
doppler << "Hz tr " << tr<< " cl-count " << (unsigned int)plaintext[offset+16]<< " pl-count " << plcount;
|
|
cerr<<" tau " << ldexp((tr - maxtr), -32)/1000.0;
|
|
cerr<<" pr " << 3e5*ldexp((tr - maxtr), -32)/1000.0;
|
|
cerr<<endl;
|
|
*/
|
|
if((gnssid== 0 || gnssid == 2) && plcount > 2) {
|
|
struct TrkSatStat tss;
|
|
tss.sv = sv;
|
|
tss.gnss = gnssid;
|
|
tss.dopplerHz = doppler;
|
|
tss.tr = tr;
|
|
ret.push_back(tss);
|
|
}
|
|
|
|
}
|
|
/*
|
|
for(int n = 0 ; n < plaintext[8]; ++n) {
|
|
int offset = 110+n*56;
|
|
cerr<<" gnssid " << (unsigned int)plaintext[offset+4]<<" sv "<<(unsigned int) plaintext[offset+5]<<endl;
|
|
for(int j = 0; j < 56; ++j) {
|
|
cerr<< fmt::sprintf("%02x ", (unsigned int)plaintext[offset+j]);
|
|
if((j % 16)==15)
|
|
cerr<<endl;
|
|
}
|
|
cerr<<endl;
|
|
}
|
|
*/
|
|
return ret;
|
|
}
|