add BeiDou D2 messages, speed up some things, add Beidou parity checking
parent
bca8a0b2b2
commit
cbab861403
44
beidou.cc
44
beidou.cc
|
@ -1,13 +1,57 @@
|
|||
#include "beidou.hh"
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
using namespace std;
|
||||
|
||||
// with immense gratitude to https://stackoverflow.com/questions/24612436/how-to-check-a-bch15-11-1-code-checksum-for-bds-beidou-satellite-system
|
||||
|
||||
static int checkbds(int bits)
|
||||
{
|
||||
static int const at[15] = {1, 2, 4, 8, 3, 6, 12, 11, 5, 10, 7, 14, 15, 13, 9};
|
||||
int s, i, j;
|
||||
|
||||
for(i = 1; i <= 2; i++)
|
||||
{
|
||||
s = 0;
|
||||
for(j = 0; j < 15; j++)
|
||||
{
|
||||
if(bits & (1<<j))
|
||||
{
|
||||
s ^= at[(i * j) % 15];
|
||||
}
|
||||
}
|
||||
if(s != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload)
|
||||
{
|
||||
|
||||
// payload consists of 32 bit words where we have to ignore the first 2 bits of every word
|
||||
|
||||
int chunk = getbitu(&payload[0], 17, 15);
|
||||
// cout <<"w0 checkbds(chunk0): "<<checkbds(chunk) << endl;
|
||||
if(!checkbds(chunk))
|
||||
throw std::runtime_error("Beidou checksum error chunk0");
|
||||
|
||||
uint8_t buffer[(26+9*22)/8];
|
||||
|
||||
setbitu(buffer, 0, 26, getbitu(&payload[0], 2, 26));
|
||||
|
||||
for(int w = 1 ; w < 10; ++w) {
|
||||
int chunk1 = getbitu(&payload[0], 2+w*32, 11);
|
||||
int chunk2 = getbitu(&payload[0], 2+w*32+11, 11);
|
||||
int parity1 = getbitu(&payload[0], 2+w*32+22, 4);
|
||||
int parity2 = getbitu(&payload[0], 2+w*32+22+4, 4);
|
||||
chunk1 = (chunk1<<4) | parity1;
|
||||
chunk2 = (chunk2<<4) | parity2;
|
||||
//cout <<"w"<<w<<" checkbds(chunk1): "<<checkbds(chunk1) << ", checkbds(chunk2): "<<checkbds(chunk2)<<endl;
|
||||
if(!checkbds(chunk1) || !checkbds(chunk2))
|
||||
throw std::runtime_error("Beidou checksum error");
|
||||
setbitu(buffer, 26+22*(w-1), 22, getbitu(&payload[0], 2 + w*32, 22));
|
||||
}
|
||||
|
||||
|
|
10
beidou.hh
10
beidou.hh
|
@ -8,6 +8,16 @@
|
|||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload);
|
||||
int beidouBitconv(int their);
|
||||
|
||||
/* Geostationary, so D2, so not to be parsed by this parser:
|
||||
C01 (140E)
|
||||
C02 (80E)
|
||||
C03 (110.5E)
|
||||
C04 (160E)
|
||||
C05 (58.75E)
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
struct BeidouMessage
|
||||
{
|
||||
|
|
81
navdump.cc
81
navdump.cc
|
@ -36,14 +36,43 @@ static std::string humanTime(time_t t)
|
|||
}
|
||||
|
||||
|
||||
string beidouHealth(int in)
|
||||
{
|
||||
string ret;
|
||||
if(in == 256) {
|
||||
return "NO CLOCK";
|
||||
}
|
||||
if(in==511) {
|
||||
return "NO SAT";
|
||||
}
|
||||
|
||||
if(in & (1<<7))
|
||||
ret += "B1I abnormal ";
|
||||
if(in & (1<<6))
|
||||
ret += "B2I abnormal ";
|
||||
if(in & (1<<5))
|
||||
ret += "B3I abnormal ";
|
||||
if(in & (1<<1))
|
||||
ret += "navigation abnormal ";
|
||||
|
||||
if(ret.empty())
|
||||
return "OK";
|
||||
return ret;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
for(;;) {
|
||||
char bert[4];
|
||||
if(read(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') {
|
||||
cerr<<"EOF or bad magic"<<endl;
|
||||
int res = read(0, bert, 4);
|
||||
if( res != 4) {
|
||||
cerr<<"EOF, res = "<<res<<endl;
|
||||
break;
|
||||
}
|
||||
if(bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') {
|
||||
cerr<<"Bad magic"<<endl;
|
||||
}
|
||||
|
||||
|
||||
uint16_t len;
|
||||
if(read(0, &len, 2) != 2)
|
||||
|
@ -58,7 +87,7 @@ int main(int argc, char** argv)
|
|||
cout<<humanTime(nmm.localutcseconds())<<" "<<nmm.localutcnanoseconds()<<" ";
|
||||
cout<<"src "<<nmm.sourceid()<< " ";
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<endl;
|
||||
cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<", db "<<nmm.rd().db()<<" ele "<<nmm.rd().el() <<" azi "<<nmm.rd().azi()<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
|
@ -109,17 +138,51 @@ int main(int argc, char** argv)
|
|||
}
|
||||
cout<<"\n";
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::BeidouInavType) {
|
||||
int sv = nmm.bi().gnsssv();
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bi().contents().c_str(), nmm.bi().contents().size()));
|
||||
else if(nmm.type() == NavMonMessage::BeidouInavTypeD1) {
|
||||
int sv = nmm.bid1().gnsssv();
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
BeidouMessage bm;
|
||||
uint8_t pageno;
|
||||
int fraid = bm.parse(cond, &pageno);
|
||||
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;
|
||||
if(fraid == 1) {
|
||||
cout<<" wn "<<bm.wn<<" t0c "<<(int)bm.t0c<<" aodc "<< (int)bm.aodc <<" aode "<< (int)bm.aode <<" sath1 "<< (int)bm.sath1 << " urai "<<(int)bm.urai << " af0 "<<bm.a0 <<" af1 " <<bm.a1<<endl;
|
||||
}
|
||||
else if(fraid == 4 && 1<= pageno && pageno <= 24) {
|
||||
cout <<" pageno "<< (int) pageno<<" AmEpID "<< getbitu(&cond[0], beidouBitconv(291), 2);
|
||||
}
|
||||
else if(fraid == 5 && 1<= pageno && pageno <= 6) {
|
||||
cout <<" pageno "<<(int)pageno<<" AmEpID "<< getbitu(&cond[0], beidouBitconv(291), 2);
|
||||
}
|
||||
|
||||
else if(fraid == 5 && pageno==7) {
|
||||
for(int n=0; n<19; ++n)
|
||||
cout<<" hea"<<(1+n)<<" " << getbitu(&cond[0], beidouBitconv(51+n*9), 9) << " ("<<beidouHealth(getbitu(&cond[0], beidouBitconv(51+n*9), 9)) <<")";
|
||||
}
|
||||
|
||||
else if(fraid == 5 && pageno==8) {
|
||||
for(int n=0; n<10; ++n)
|
||||
cout<<" hea"<<(20+n)<<" " << getbitu(&cond[0], beidouBitconv(51+n*9), 9) << " ("<<beidouHealth(getbitu(&cond[0], beidouBitconv(51+n*9), 9))<<")";
|
||||
cout<<" WNa "<<getbitu(&cond[0], beidouBitconv(190), 8)<<" t0a "<<getbitu(&cond[0], beidouBitconv(198), 8);
|
||||
}
|
||||
else if(fraid == 5 && pageno==24) {
|
||||
int AmID= getbitu(&cond[0], beidouBitconv(216), 2);
|
||||
cout<<" AmID "<< AmID;
|
||||
for(int n=0; n<14; ++n)
|
||||
cout<<" hea"<<(31+n)<<" (" << getbitu(&cond[0], beidouBitconv(51+n*9), 9) << " "<<beidouHealth(getbitu(&cond[0], beidouBitconv(51+n*9), 9))<<")";
|
||||
}
|
||||
cout<<endl;
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::BeidouInavTypeD2) {
|
||||
int sv = nmm.bid2().gnsssv();
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
BeidouMessage bm;
|
||||
uint8_t pageno;
|
||||
int fraid = bm.parse(cond, &pageno);
|
||||
|
||||
cout<<"BeiDou "<<sv<<" D2: "<<bm.sow<<", FraID "<<fraid << endl;
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
cout<<"ECEF"<<endl;
|
||||
|
|
18
navmon.proto
18
navmon.proto
|
@ -7,8 +7,9 @@ message NavMonMessage {
|
|||
GalileoInavType = 3;
|
||||
RFDataType = 4;
|
||||
GPSInavType = 5;
|
||||
BeidouInavType = 6;
|
||||
GLONASSInavType = 7;
|
||||
BeidouInavTypeD1 = 6;
|
||||
GLONASSInavType = 7;
|
||||
BeidouInavTypeD2 = 8;
|
||||
}
|
||||
|
||||
required uint64 sourceID = 1;
|
||||
|
@ -35,7 +36,15 @@ message NavMonMessage {
|
|||
required bytes contents =5;
|
||||
}
|
||||
|
||||
message BeidouInav {
|
||||
message BeidouInavD1 {
|
||||
required uint32 gnssWN =1;
|
||||
required uint32 gnssTOW =2; // INTEGERS!
|
||||
|
||||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
}
|
||||
message BeidouInavD2 {
|
||||
required uint32 gnssWN =1;
|
||||
required uint32 gnssTOW =2; // INTEGERS!
|
||||
|
||||
|
@ -81,5 +90,6 @@ message NavMonMessage {
|
|||
optional RFData rfd=7;
|
||||
optional ObserverPosition op=8;
|
||||
optional GPSInav gpsi=9;
|
||||
optional BeidouInav bi=10;
|
||||
optional BeidouInavD1 bid1=10;
|
||||
optional BeidouInavD2 bid2=11;
|
||||
}
|
||||
|
|
43
ubxtool.cc
43
ubxtool.cc
|
@ -188,16 +188,11 @@ std::pair<UBXMessage, struct timeval> getUBXMessage(int fd)
|
|||
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
|
||||
if(!g_fromFile)
|
||||
writen2(logfile, msg.c_str(), msg.size());
|
||||
|
@ -260,10 +255,13 @@ void emitNMM(int fd, const NavMonMessage& nmm)
|
|||
|
||||
string out;
|
||||
nmm.SerializeToString(& out);
|
||||
writen2(fd, "bert", 4);
|
||||
string msg("bert");
|
||||
|
||||
uint16_t len = htons(out.size());
|
||||
writen2(fd, &len, 2);
|
||||
writen2(fd, out.c_str(), out.size());
|
||||
msg.append((char*)&len, 2);
|
||||
msg.append(out);
|
||||
|
||||
writen2(fd, msg.c_str(), msg.size());
|
||||
}
|
||||
|
||||
void enableUBXMessageUSB(int fd, uint8_t ubxClass, uint8_t ubxType, uint8_t rate=1)
|
||||
|
@ -621,9 +619,9 @@ int main(int argc, char** argv)
|
|||
};
|
||||
pos p;
|
||||
memcpy(&p, payload.c_str(), sizeof(pos));
|
||||
cerr<<"Position: ("<< p.ecefX / 100000.0<<", "
|
||||
/* cerr<<"Position: ("<< p.ecefX / 100000.0<<", "
|
||||
<< p.ecefY / 100000.0<<", "
|
||||
<< p.ecefZ / 100000.0<<") +- "<<p.pAcc<<" cm"<<endl;
|
||||
<< p.ecefZ / 100000.0<<") +- "<<p.pAcc<<" cm"<<endl;*/
|
||||
|
||||
// g_ourpos = {p.ecefX/100.0, p.ecefY/100.0, p.ecefZ/100.0};
|
||||
|
||||
|
@ -746,15 +744,26 @@ int main(int argc, char** argv)
|
|||
continue;
|
||||
}
|
||||
NavMonMessage nmm;
|
||||
nmm.set_type(NavMonMessage::BeidouInavType);
|
||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||
nmm.set_sourceid(g_srcid);
|
||||
nmm.mutable_bi()->set_gnsswn(bm.wn);
|
||||
nmm.mutable_bi()->set_gnsstow(bm.sow);
|
||||
nmm.mutable_bi()->set_gnssid(id.first);
|
||||
nmm.mutable_bi()->set_gnsssv(id.second);
|
||||
nmm.mutable_bi()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
nmm.set_sourceid(g_srcid);
|
||||
if(id.second > 5) {
|
||||
// this **HARDCODES** that C01,02,03,04,05 emit D2 messages!
|
||||
nmm.set_type(NavMonMessage::BeidouInavTypeD1);
|
||||
nmm.mutable_bid1()->set_gnsswn(bm.wn);
|
||||
nmm.mutable_bid1()->set_gnsstow(bm.sow);
|
||||
nmm.mutable_bid1()->set_gnssid(id.first);
|
||||
nmm.mutable_bid1()->set_gnsssv(id.second);
|
||||
nmm.mutable_bid1()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
}
|
||||
else {
|
||||
nmm.set_type(NavMonMessage::BeidouInavTypeD2);
|
||||
nmm.mutable_bid2()->set_gnsswn(bm.wn);
|
||||
nmm.mutable_bid2()->set_gnsstow(bm.sow);
|
||||
nmm.mutable_bid2()->set_gnssid(id.first);
|
||||
nmm.mutable_bid2()->set_gnsssv(id.second);
|
||||
nmm.mutable_bid2()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
}
|
||||
emitNMM(1, nmm);
|
||||
continue;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue